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

13 files changed, 1577 insertions, 948 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e372541..9aedf74 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,17 +1,27 @@
-add_executable(ads
-               active_drag_system.cpp
-               spi_flash.c
-               imu.cpp
-               pwm.cpp
-               altimeter.cpp
-               kalman_filter.cpp
+add_executable(active_drag_system
+               ${PROJECT_SOURCE_DIR}/src/active_drag_system.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/pwm.cpp
+               ${PROJECT_SOURCE_DIR}/src/log_format.cpp
+               ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp
+               ${PROJECT_SOURCE_DIR}/src/serial.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)
+pico_set_binary_type(active_drag_system copy_to_ram)
 
-pico_enable_stdio_usb(ads 0)
-pico_enable_stdio_uart(ads 0)
+target_link_libraries(active_drag_system pico_stdlib pico_logger pico_flash pico_rand pico_multicore pico_sync hardware_i2c hardware_adc hardware_timer hardware_pwm FreeRTOS-Kernel FreeRTOS-Kernel-Heap4 libfixmath libfixmatrix libfixkalman)
+target_include_directories(active_drag_system PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_compile_definitions(active_drag_system PRIVATE
+        USE_FREERTOS=1
+        #        DEBUG=1
+        PICO_STDIO_STACK_BUFFER_SIZE=64 # use a small printf on stack buffer
+)
+
+pico_enable_stdio_usb(active_drag_system 1)
+pico_enable_stdio_uart(active_drag_system 0)
 
-pico_add_extra_outputs(ads)
+pico_add_extra_outputs(active_drag_system)
 
diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp
index 6f5d57e..ce898bb 100644
--- a/src/active_drag_system.cpp
+++ b/src/active_drag_system.cpp
@@ -1,436 +1,845 @@
-#include "cyw43_configport.h"
-#include "pico/multicore.h"
-#include "pico/platform.h"
-#include "pico/sem.h"
-#include "boards/pico_w.h"
+#include <pico.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+
 #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/types.h"
-#include "pico/cyw43_arch.h"
-#include <inttypes.h>
-#include <math.h>
+#include <pico/error.h>
+#include <pico/types.h>
+#include "math.h"
+
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "portmacro.h"
+#include "projdefs.h"
+#include "serial.hpp"
+#include "task.h"
+#include "semphr.h"
+
+extern "C" {
+#include <fix16.h>
+#include <fixmatrix.h>
+#include <fixkalman.h>
+}
 
+#include "adxl375.hpp"
+#include "ms5607.hpp"
+#include "iim42653.hpp"
+#include "mmc5983ma.hpp"
 #include "pwm.hpp"
-#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 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 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 = 0,
-    BOOST,
-    COAST,
-    APOGEE,
-    RECOVERY,
-    END
-} state_t;
+#include "pico_logger.h"
+#include "log_format.hpp"
+#include "serial.hpp"
+#include "heartbeat.hpp"
+
+/****************************** FREERTOS **************************************/
+#define SENSOR_EVENT_HANDLER_PRIORITY   ( tskIDLE_PRIORITY + 7 )
+#define SENSOR_SAMPLE_PRIORITY          ( tskIDLE_PRIORITY + 6 )
+#define KALMAN_TASK_PRIORITY            ( tskIDLE_PRIORITY + 6 )
+#define ROCKET_TASK_PRIORITY            ( tskIDLE_PRIORITY + 4 )
+#define ROCKET_EVENT_HANDLER_PRIORITY   ( tskIDLE_PRIORITY + 3 )
+#define	HEARTBEAT_TASK_PRIORITY		    ( tskIDLE_PRIORITY + 2 )
+#define LOGGING_PRIORITY                ( tskIDLE_PRIORITY + 2 )
+#define	SERIAL_TASK_PRIORITY		    ( tskIDLE_PRIORITY + 1 )
+
+void vApplicationTickHook(void) { /* optional */ }
+void vApplicationMallocFailedHook(void) { /* optional */ }
+void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { for( ;; ); }
+
+static void logging_task(void* pvParameters);
+
+#define ROCKET_TASK_RATE_HZ 50
+volatile TaskHandle_t rocket_task_handle = NULL;
+static void rocket_task(void* pvParameters);
+
+#define KALMAN_TASK_RATE_HZ 50
+volatile TaskHandle_t kalman_task_handle = NULL;
+static void kalman_task(void* pvParameters);
+
+int64_t launch_event_callback(alarm_id_t id, void* user_data);
+int64_t coast_event_callback(alarm_id_t id, void* user_data);
+int64_t end_event_callback(alarm_id_t id, void* user_data);
+
+volatile TaskHandle_t launch_event_handler_task_handle = NULL;
+volatile TaskHandle_t coast_event_handler_task_handle = NULL;
+volatile TaskHandle_t end_event_handler_task_handle = NULL;
+static void launch_event_handler_task(void* pvParameters);
+static void coast_event_handler_task(void* pvParameters);
+static void end_event_handler_task(void* pvParameters);
+
+/****************************** FREERTOS **************************************/
+
+/****************************** SERIAL CONSOLE ********************************/
+static void read_cmd_func();
+static void write_cmd_func();
+static void erase_cmd_func();
+static void show_cmd_func();
+static void deploy_cmd_func();
+static void kalman_cmd_func();
+
+const char* executeable_name = "active-drag-system.uf2";
+const size_t num_user_cmds = 6;
+const command_t user_commands[] = { {.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 = "show",
+                                     .len = 4,
+                                     .function = &show_cmd_func},
+                                    {.name = "deploy",
+                                     .len = 6,
+                                     .function = &deploy_cmd_func},
+                                    {.name = "kalman",
+                                     .len = 6,
+                                     .function = &kalman_cmd_func} };
+/****************************** SERIAL CONSOLE ********************************/
+
+/****************************** LOGGING ***************************************/
+volatile bool use_circular_buffer = true;
+volatile TaskHandle_t logging_handle = NULL;
+volatile log_entry_t log_entry;
+
+Logger logger(PACKET_SIZE, LOG_BASE_ADDR, print_log_entry);
+
+static void populate_log_entry(log_entry_t* log_entry);
+/****************************** LOGGING ***************************************/
+
+volatile bool serial_data_output = false;
+
+MS5607 alt(i2c_default);
+ADXL375 adxl375(i2c_default);
+IIM42653 iim42653(i2c_default);
+MMC5983MA mmc5983ma(i2c_default);
 
 PWM pwm;
-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);
-float get_deploy_percent(float velocity, float altitude);
-
-bool timer_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 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;
 
-Eigen::Vector3f linear_acceleration;
-Eigen::Vector4f quaternion;
-Eigen::Vector3f euler_angles;
-
-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;
+#define MOTOR_BURN_TIME 6200
+volatile state_t rocket_state = PAD;
+volatile uint8_t deployment_percent = 0;
 
-altimeter altimeter(i2c_default, MPL3115A2_ADDR);
-imu imu(i2c_default, BNO055_ADDR, BNO055_ID, NDOF);
+volatile int32_t ground_altitude = 0;
+volatile int32_t threshold_altitude = 30;
+
+/****************************** KALMAN ****************************************/
+#define KALMAN_NAME verticality
+#define KALMAN_NUM_STATES 2
+#define KALMAN_NUM_INPUTS 1
+kalman16_t kf;
+
+#define KALMAN_MEASUREMENT_NAME altitude
+#define KALMAN_NUM_MEASUREMENTS 1
+kalman16_observation_t kfm;
+
+#define matrix_set(matrix, row, column, value) \
+    matrix->data[row][column] = value
+
+#define matrix_set_symmetric(matrix, row, column, value) \
+    matrix->data[row][column] = value; \
+    matrix->data[column][row] = value
+
+#ifndef FIXMATRIX_MAX_SIZE
+#error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements.
+#endif
+
+#if (FIXMATRIX_MAX_SIZE < KALMAN_NUM_STATES) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_INPUTS) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_MEASUREMENTS)
+#error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements.
+#endif
+
+static void kalman_verticality_init() {
+    /************************************************************************/
+    /* initialize the filter structures                                     */
+    /************************************************************************/
+    kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
+    kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);
+
+    /************************************************************************/
+    /* set initial state                                                    */
+    /************************************************************************/
+    mf16 *x = kalman_get_state_vector(&kf);
+    x->data[0][0] = 0; // s_i
+    x->data[1][0] = 0; // v_i
+
+    /************************************************************************/
+    /* set state transition                                                 */
+    /************************************************************************/
+    mf16 *A = kalman_get_state_transition(&kf);
+    
+    // set time constant
+#if (DEBUG == 1)
+    const fix16_t T = fix16_from_float(0.02f);
+#else
+    const fix16_t T = fix16_from_float(0.02f);
+#endif
+    const fix16_t Tsquare = fix16_sq(T);
+
+    // helper
+    const fix16_t fix16_half = fix16_from_float(0.5);
+
+    // transition of x to s
+    matrix_set(A, 0, 0, fix16_one);   // 1
+    matrix_set(A, 0, 1, T);   // T
+    
+    // transition of x to v
+    matrix_set(A, 1, 0, 0);   // 0
+    matrix_set(A, 1, 1, fix16_one);   // 1
+
+
+    /************************************************************************/
+    /* set covariance                                                       */
+    /************************************************************************/
+    mf16 *P = kalman_get_system_covariance(&kf);
+
+//    matrix_set_symmetric(P, 0, 0, fix16_half);   // var(s)
+//    matrix_set_symmetric(P, 0, 1, 0);   // cov(s,v)
+//    matrix_set_symmetric(P, 0, 2, 0);   // cov(s,g)
+//
+//    matrix_set_symmetric(P, 1, 1, fix16_one);   // var(v)
+//    matrix_set_symmetric(P, 1, 2, 0);   // cov(v,g)
+//
+//    matrix_set_symmetric(P, 2, 2, fix16_one);   // var(g)
+
+    /************************************************************************/
+    /* set input covariance                                                 */
+    /************************************************************************/
+    mf16 *Q = kalman_get_input_covariance(&kf);
+//    mf16_fill_diagonal(Q, fix16_one);
+    mf16_mul_bt(Q, A, A);
+    mf16_mul_s(Q, Q, fix16_from_int(10*10));
+
+    /************************************************************************/
+    /* set control input transformation                                       */
+    /************************************************************************/
+    mf16 *B = kalman_get_input_transition(&kf);
+    matrix_set(B, 0, 0, fix16_mul(fix16_half, Tsquare)); // u = 0*s 
+    matrix_set(B, 1, 0, T);                              //   + 0*v
+
+    /************************************************************************/
+    /* set measurement transformation                                       */
+    /************************************************************************/
+    mf16 *H = kalman_get_observation_transformation(&kfm);
+    matrix_set(H, 0, 0, fix16_one);     // z = 1*s 
+    matrix_set(H, 0, 1, 0);             //   + 0*v
+
+    /************************************************************************/
+    /* set process noise                                                    */
+    /************************************************************************/
+    mf16 *R = kalman_get_observation_process_noise(&kfm);
+
+    matrix_set(R, 0, 0, fix16_from_int(36));     // var(s)
+}
 
-uint8_t *altimeter_buffer;
-uint8_t *acceleration_buffer;
-uint8_t *quaternion_buffer;
+volatile bool derate_baro_sensor = false;
+const fix16_t baro_velocity_derate = F16(160.f);
+
+volatile fix16_t altitude_filt = 0;
+volatile fix16_t velocity_filt = 0;
+
+volatile fix16_t drag_force = 0;
+volatile fix16_t apogee_prediction = 0;
+volatile fix16_t desired_drag_force = 0;
+volatile fix16_t desired_deployment = 0;
+
+void kalman_update(fix16_t altitude, fix16_t vertical_acceleration) {
+    static mf16* control = kalman_get_input_vector(&kf);
+    static mf16* measurement = kalman_get_observation_vector(&kfm);
+    static mf16* Q = kalman_get_input_covariance(&kf);
+    static mf16 *R = kalman_get_observation_process_noise(&kfm);
+    static mf16* state_vector = kalman_get_state_vector(&kf);
+
+    if (state_vector->data[1][0] >= baro_velocity_derate && !derate_baro_sensor) {
+        mf16_div_s(Q, Q, fix16_from_int(3));
+        mf16_mul_s(R, R, fix16_from_int(4));
+        derate_baro_sensor = true;
+    } else if (state_vector->data[1][0] < baro_velocity_derate && derate_baro_sensor) {
+        mf16_mul_s(Q, Q, fix16_from_int(3));
+        mf16_div_s(R, R, fix16_from_int(4));
+        derate_baro_sensor = false;
+    }
+    matrix_set(control, 0, 0, vertical_acceleration);
+    kalman_predict(&kf);
+    matrix_set(measurement, 0, 0, altitude);
+    kalman_correct(&kf, &kfm);
+}
 
-uint8_t entry_buffer[PACKET_SIZE];
+fix16_t calculate_drag_force(fix16_t deployment_percentage, fix16_t vertical_velocity);
+fix16_t predict_apogee(fix16_t altitude, fix16_t vertical_velocity, fix16_t drag_force);
+fix16_t calculate_deployment_percentage(fix16_t drag_force, fix16_t vertical_velocity);
+fix16_t calculate_desired_drag_force(fix16_t altitude, fix16_t vertical_velocity);
 
-uint8_t *pad_buffer;
+/****************************** KALMAN ****************************************/
 
 int main() {
+    stdio_init_all();
+
     adc_init();
     adc_set_temp_sensor_enabled(true);
 
-    cyw43_arch_init();
+    heartbeat_initialize(PICO_DEFAULT_LED_PIN);
 
-    i2c_init(i2c_default, MAX_SCL);
+    i2c_init(i2c_default, 400000);
     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);
-
-    // Enable SPI 0 at 60 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);
-
-    gpio_init(INT1_PIN);
-    gpio_pull_up(INT1_PIN);
-    
-    alarm_pool_init_default();
 
-    altimeter.initialize(30.0f, INT1_PIN, &pad_callback);
+    gpio_init(MICRO_DEFAULT_SERVO_ENABLE);
+    gpio_set_dir(MICRO_DEFAULT_SERVO_ENABLE, GPIO_OUT);
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0);
+
+    sleep_ms(2500);
+
+    info_cmd_func();
+    stdio_flush();
 
-    imu.initialize();
-    imu.linear_acceleration(linear_acceleration);
-    imu.quaternion(quaternion);
-    imu.quaternion_euler(euler_angles, quaternion);
+    logger.initialize(true);
+    logger.initialize_circular_buffer(PAD_BUFFER_SIZE);
 
+    alt.initialize();
+    sleep_ms(500);
+    adxl375.initialize();
+    sleep_ms(500);
+    iim42653.initialize();
+    sleep_ms(500);
+    mmc5983ma.initialize();
+    sleep_ms(500);
     pwm.init();
 
-    // Initialize MOSFET
-    gpio_init(MOSFET_PIN);
-    gpio_set_dir(MOSFET_PIN, GPIO_OUT);
+    kalman_verticality_init();
 
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
+    xTaskCreate(heartbeat_task, "heartbeat", 256, NULL, HEARTBEAT_TASK_PRIORITY, NULL);
+    xTaskCreate(serial_task, "serial", 8192, NULL, SERIAL_TASK_PRIORITY, NULL);
 
-    pad_buffer = (uint8_t*)malloc(PAD_BUFFER_SIZE);
+    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));
 
-    // 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();
+    xTaskCreate(MS5607::ms5607_sample_handler, "ms5607_sample_handler", 256, &alt, SENSOR_EVENT_HANDLER_PRIORITY, &(alt.sample_handler_task_handle));
 
-    altimeter.expose_buffer(&altimeter_buffer);
-    imu.expose_acceleration_buffer(&acceleration_buffer);
-    imu.expose_quaternion_buffer(&quaternion_buffer);
+    xTaskCreate(rocket_task, "rocket_task", 512, NULL, SENSOR_SAMPLE_PRIORITY, const_cast<TaskHandle_t *>(&rocket_task_handle));
+#if (DEBUG != 1)
+    xTaskCreate(kalman_task, "kalman_task", 512, NULL, KALMAN_TASK_PRIORITY, const_cast<TaskHandle_t *>(&kalman_task_handle));
+    vTaskCoreAffinitySet(kalman_task_handle, 0x01);
+#endif
+    vTaskCoreAffinitySet( alt.update_task_handle, 0x01 );
+    vTaskCoreAffinitySet( adxl375.update_task_handle, 0x01 );
+    vTaskCoreAffinitySet( iim42653.update_task_handle, 0x01 );
+    vTaskCoreAffinitySet( mmc5983ma.update_task_handle, 0x01 );
 
-    sem_init(&sem, 1, 1);
+    vTaskCoreAffinitySet(rocket_task_handle, 0x01);
 
-    add_repeating_timer_us(-1000000 / DATA_RATE_HZ,  &timer_callback, NULL, &data_timer);
+    vTaskCoreAffinitySet( alt.sample_handler_task_handle, 0x01 );
 
-    multicore_launch_core1(logging_core);
+    xTaskCreate(logging_task, "logging", 256, NULL, LOGGING_PRIORITY, const_cast<TaskHandle_t *>(&logging_handle));
+    vTaskCoreAffinitySet(logging_handle, 0x02);
+
+    vTaskStartScheduler();
 
     while (1) {
         tight_loop_contents();
     }
 }
 
-// PRIMARY THREAD RELATED FUNCTIONS AND CALLBACKS
-//===============================================================================
-
-bool timer_callback(repeating_timer_t *rt) {
-    sem_acquire_blocking(&sem);
-    imu.linear_acceleration(linear_acceleration);
-    imu.quaternion(quaternion);
-
-    control(0) = linear_acceleration.z();
-    measurement(0) = altimeter.get_altitude_converted();
-    res = kf->run(control, measurement, 0.01f);
-    altitude = res(0);
-    velocity = res(1);
-
-
-    deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, (altitude - ground_altitude))), 100.0f));
-
-    switch(state) {
-        case PAD:
-            gpio_put(MOSFET_PIN, 0);
-            pwm.set_servo_percent(0);
-            deployment_percent = 0;
-            break;
-        case BOOST:
-            gpio_put(MOSFET_PIN, 1);
-            pwm.set_servo_percent(0);
-            deployment_percent = 0;
-            break;
-        case COAST:
-            gpio_put(MOSFET_PIN, 1);
-            pwm.set_servo_percent(deployment_percent);
-            break;
-        case APOGEE:
-            gpio_put(MOSFET_PIN, 1);
-            pwm.set_servo_percent(0);
-            deployment_percent = 0;
-            break;
-        case RECOVERY:
-            gpio_put(MOSFET_PIN, 1);
-            pwm.set_servo_percent(0);
-            deployment_percent = 0;
-            break;
-        case END:
-            gpio_put(MOSFET_PIN, 1);
-            pwm.set_servo_percent(0);
-            deployment_percent = 0;
-            break;
-    }
-    sem_release(&sem);
-    return true;
+/****************************** LOGGING ***************************************/
+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 = rocket_state;
+    log_entry->deploy_percent = deployment_percent;
+    log_entry->pressure = alt.get_pressure();
+    log_entry->altitude = alt.get_altitude();
+    log_entry->altitude_filt = altitude_filt;
+    log_entry->velocity_filt = velocity_filt;
+    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->drag_force = drag_force;
+    log_entry->apogee_prediction = apogee_prediction;
+    log_entry->desired_drag_force = desired_drag_force;
+
+    log_entry->data0 = get_rand_32();
 }
 
-/**
- * @brief Call back function for when rocket is on the pad
- * 
- * @param gpio pin number of interrupt
- * @param event_mask interrupt condition, value is set by PICO_SDK
- *  GPIO_IRQ_LEVEL_LOW = 0x1u,
- *  GPIO_IRQ_LEVEL_HIGH = 0x2u,
- *  GPIO_IRQ_EDGE_FALL = 0x4u,
- *  GPIO_IRQ_EDGE_RISE = 0x8u,
- * @link https://www.raspberrypi.com/documentation/pico-sdk/hardware/gpio.html#ga6347e27da3ab34f1ea65b5ae16ab724f
- */
-void pad_callback(uint gpio, uint32_t event_mask) {
-    sem_acquire_blocking(&sem);
-    altimeter.unset_threshold_altitude(INT1_PIN);
-    state = BOOST;
-    sem_release(&sem);
-    // start motor burn timer with boost transition function as callback
-    add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false);
+static void logging_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / LOG_RATE_HZ);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        populate_log_entry(const_cast<log_entry_t *>(&log_entry));
+        if (serial_data_output) {
+            print_log_entry(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)));
+            stdio_flush();
+        }
+
+        if (use_circular_buffer) {
+            logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)));
+            if (rocket_state != PAD) {
+                logger.flush_circular_buffer(true);
+                use_circular_buffer = false;
+            }
+        } else {
+            logger.write_memory(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)), false);
+        }
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
 }
+/****************************** LOGGING ***************************************/
 
-int64_t boost_callback(alarm_id_t id, void* user_data) {
-    // Configure accelerometer and/or altimeter to generate interrupt
-    // for when velocity is negative with this function as callback to
-    // transition to APOGEE
-    sem_acquire_blocking(&sem);
-    state = COAST;
-    populate_entry();
-    write_entry(entry_buffer);
-    sem_release(&sem);
-    add_alarm_in_ms(1000, &coast_callback, NULL, false);
-    return 0;
+/****************************** SERIAL CONSOLE ********************************/
+static void read_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    logger.read_memory();
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
 }
 
-int64_t coast_callback(alarm_id_t id, void* user_data) {
-    // Want to somehow immediately transition to RECOVERY from APOGEE (extremely short timer?)
-    if (velocity <= 0.0f) {
-        sem_acquire_blocking(&sem);
-        state = APOGEE;
-        populate_entry();
-        write_entry(entry_buffer);
-        sem_release(&sem);
-        add_alarm_in_ms(1, &apogee_callback, NULL, false);
+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 {
-        add_alarm_in_ms(250, &coast_callback, NULL, false);
+        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);
     }
-    return 0;
 }
 
-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
-    altimeter.set_threshold_altitude((ground_altitude + 10.0f), INT1_PIN, &recovery_callback);
+static void erase_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    logger.erase_memory();
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
 
-    sem_acquire_blocking(&sem);
-    populate_entry();
-    write_entry(entry_buffer);
-    sem_release(&sem);
-    return 0;
+static void show_cmd_func() {
+    serial_data_output = !serial_data_output;
+}
+
+static void deploy_cmd_func() {
+    vTaskSuspend(rocket_task_handle);
+    printf("Enabling servo!\n");
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    printf("Setting servo to 80%\n");
+    pwm.set_servo_percent(80);
+    vTaskDelay(pdMS_TO_TICKS(5000));
+    printf("Setting servo to 0%\n");
+    pwm.set_servo_percent(0);
+    vTaskDelay(pdMS_TO_TICKS(5000));
+    printf("Setting servo to 80%\n");
+    pwm.set_servo_percent(80);
+    vTaskDelay(pdMS_TO_TICKS(5000));
+    printf("Setting servo to 0%\n");
+    pwm.set_servo_percent(0);
+    vTaskDelay(pdMS_TO_TICKS(5000));
+    printf("Disabling servo!\n");
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0);
+    vTaskResume(rocket_task_handle);
 }
 
-void recovery_callback(uint gpio, uint32_t event_mask) {
-    // Essentially just a signal to stop logging data
-    sem_acquire_blocking(&sem);
-    state = END;
-    populate_entry();
-    write_entry(entry_buffer);
-    sem_acquire_blocking(&sem);
+#if (DEBUG == 1)
+#include "ohio_test_data.h"
+#endif
+
+static void kalman_cmd_func() {
+    static mf16* state_vector = kalman_get_state_vector(&kf);
+    printf("Perfoming Kalman Filter Test! Stand back!\n");
+    printf("*******************************************\n");
+    printf("\naltitude,velocity,acceleration,drag_force,apogee_prediction,desired_drag_force,desired_deployment\n");
+
+    fix16_t drag_force_l = 0;
+    fix16_t apogee_prediction_l = 0;
+    fix16_t desired_drag_force_l = 0;
+    fix16_t desired_deployment_l = 0;
+#if (DEBUG == 1)
+    for (uint32_t i = 0; i < 7500; i++) {
+        kalman_update(altitude_test_data[i], fix16_mul(fix16_sub(acceleration_data[i], fix16_one), F16(9.81f)));
+
+        if (i >= 693 && i <= 1632) {
+            drag_force_l = calculate_drag_force(fix16_from_int(80), state_vector->data[1][0]);
+        } else {
+            drag_force_l = calculate_drag_force(0, state_vector->data[1][0]);
+        }
+        apogee_prediction_l = predict_apogee(state_vector->data[0][0], state_vector->data[1][0], drag_force_l);
+        desired_drag_force_l = calculate_desired_drag_force(state_vector->data[0][0], state_vector->data[1][0]);
+        desired_deployment_l = calculate_deployment_percentage(desired_drag_force_l, state_vector->data[1][0]);
+        desired_deployment_l = fix16_clamp(desired_deployment_l, 0, fix16_from_int(100));
+        printf("%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n", fix16_to_float(state_vector->data[0][0]), fix16_to_float(state_vector->data[1][0]), fix16_to_float(fix16_mul(acceleration_data[i], F16(9.81f))), fix16_to_float(drag_force_l), fix16_to_float(apogee_prediction_l), fix16_to_float(desired_drag_force_l), fix16_to_float(desired_deployment_l));
+        stdio_flush();
+    }
+#else
+    kalman_update(0, 0);
+#endif
+}
+/****************************** SERIAL CONSOLE ********************************/
+
+
+static void rocket_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ROCKET_TASK_RATE_HZ);
+
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    ground_altitude = alt.get_altitude();
+
+    // Sign of life
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+    pwm.set_servo_percent(0);
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    pwm.set_servo_percent(20);
+    vTaskDelay(pdMS_TO_TICKS(3000));
+    pwm.set_servo_percent(0);
+    vTaskDelay(pdMS_TO_TICKS(3000));
+    pwm.set_servo_percent(20);
+    vTaskDelay(pdMS_TO_TICKS(3000));
+    pwm.set_servo_percent(0);
+    vTaskDelay(pdMS_TO_TICKS(3000));
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0);
+
+    xTaskCreate(launch_event_handler_task, "launch_event_handler", 512, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&launch_event_handler_task_handle));
+    vTaskCoreAffinitySet(launch_event_handler_task_handle, 0x01);
+    alt.set_threshold_altitude(ground_altitude + (threshold_altitude * ALTITUDE_SCALE), &launch_event_callback);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+
+        drag_force = calculate_drag_force(fix16_from_int(deployment_percent), velocity_filt);
+        apogee_prediction = predict_apogee(altitude_filt, velocity_filt, drag_force);
+        desired_drag_force = calculate_desired_drag_force(altitude_filt, velocity_filt);
+        desired_deployment = calculate_deployment_percentage(desired_drag_force, velocity_filt);
+        desired_deployment = fix16_clamp(desired_deployment, 0, fix16_from_int(100));
+
+        switch(rocket_state) {
+            case PAD:
+                deployment_percent = 0;
+                pwm.set_servo_percent(deployment_percent);
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0);
+                break;
+            case BOOST:
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+                deployment_percent = 0;
+                pwm.set_servo_percent(deployment_percent);
+                break;
+            case COAST:
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+                if (velocity_filt <= 0) {
+                    rocket_state = APOGEE;
+                    populate_log_entry(const_cast<log_entry_t *>(&log_entry));
+                    logger.write_memory(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)), false);
+                    deployment_percent = 0;
+                    pwm.set_servo_percent(deployment_percent);
+                    rocket_state = RECOVERY;
+                    xTaskCreate(end_event_handler_task, "end_event_handler", 1024, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&end_event_handler_task_handle));
+                    vTaskCoreAffinitySet(end_event_handler_task_handle, 0x01);
+                    add_alarm_in_ms(450000, end_event_callback, NULL, false);
+                }
+                deployment_percent = desired_deployment;
+                if ((alt.get_altitude() - ground_altitude)> (2895 * ALTITUDE_SCALE)) {
+                    deployment_percent = 100;
+                }
+                pwm.set_servo_percent(deployment_percent);
+                break;
+            case APOGEE:
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+                deployment_percent = 0;
+                pwm.set_servo_percent(deployment_percent);
+                break;
+            case RECOVERY:
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
+                deployment_percent = 0;
+                pwm.set_servo_percent(deployment_percent);
+                break;
+            case END:
+                deployment_percent = 0;
+                pwm.set_servo_percent(deployment_percent);
+                vTaskDelay(pdMS_TO_TICKS(1000));
+                gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0);
+                vTaskDelete(logging_handle);
+                vTaskDelete(end_event_handler_task_handle);
+                vTaskDelete(rocket_task_handle);
+                break;
+        }
+
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
 }
 
-/**
- * @brief Calculates the fitted Coeficient of Drag using the Surface Fit Model for the current rocket design.
- * @param velocity Velocity
- * @param altitude Altitude
- *
- * @return: Drag Coefficient (CD)
- */
-float get_deploy_percent(float velocity, float altitude) {
-    // Lookup table (Data from 'dragSurfFit.m' for Surface Fit Model Formula)
-    float p00 = -8.498e+04;
-    float p10 = 924.4;
-    float p01 = 69.98;
-    float p20 = -3.62;
-    float p11 = -0.6196;
-    float p02 = -0.01897;
-    float p30 = 0.005983;
-    float p21 = 0.001756;
-    float p12 = 0.0001271;
-    float p03 = 1.693e-06;
-    float p40 = -3.451e-06;
-    float p31 = -1.582e-06;
-    float p22 = -2.004e-07;
-    float p13 = -7.476e-09;
-
-
-    /* MATLAB Code:
-    * return p00 + p10 * V + p01 * H + p20 * V ** 2 + \
-	* p11 * V * H + p02 * H ** 2 + p30 * V ** 3 + \
-	* p21 * V ** 2 * H + p12 * V * H ** 2 + p03 * H ** 3 + \
-	* p40 * V ** 4 + p31 * V ** 3 * H + p22 * V ** 2 * H ** 2 + \
-	* p13 * V * H ** 3
-    */
-
-    return (p00 + p10 * velocity + p01 * altitude + p20 * std::pow(velocity, 2) + p11 * velocity * altitude + p02 * std::pow(altitude, 2)
-    + p30 * std::pow(velocity, 3) + p21 * std::pow(velocity, 2) * altitude + p12 * velocity * std::pow(altitude, 2) + p03 * std::pow(altitude, 3)
-    + p40 * std::pow(velocity, 4) + p31 * std::pow(velocity, 3) * altitude + p22 * std::pow(velocity, 2) * std::pow(altitude, 2) + p13 * velocity * std::pow(altitude, 3));
+static void kalman_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / KALMAN_TASK_RATE_HZ);
 
+    const fix16_t accel_scale = fix16_div(F16(9.81f), F16(S_IIM42653_ACCEL_SENSITIVITY_FACTOR));
+
+    mf16* state_vector = kalman_get_state_vector(&kf);
+    fix16_t altitude_agl = 0;
+    fix16_t vertical_accel = 1;
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        taskENTER_CRITICAL();
+        altitude_agl = fix16_div(fix16_from_int(alt.get_altitude() - ground_altitude), fix16_from_int(ALTITUDE_SCALE));
+        vertical_accel = fix16_mul(fix16_sub(fix16_from_int(iim42653.get_az()), fix16_one), accel_scale);
+        kalman_update(altitude_agl, vertical_accel);
+        altitude_filt = state_vector->data[0][0];
+        velocity_filt = state_vector->data[1][0];
+        taskEXIT_CRITICAL();
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
+}
 
+static void launch_event_handler_task(void* pvParameters) {
+    const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 1000 );
+    const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 );
+    uint32_t ulEventsToProcess;
+    while (1) {
+        /* Wait to receive a notification sent directly to this task from the
+        interrupt service routine. */
+        ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime );
+        if( ulEventsToProcess != 0 ) {
+            /* To get here at least one event must have occurred. Loop here
+            until all the pending events have been processed */
+            while( ulEventsToProcess > 0 ) {
+                vTaskSuspendAll();
+                rocket_state = BOOST;
+                xTaskCreate(coast_event_handler_task, "coast_event_handler", 512, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&coast_event_handler_task_handle));
+                vTaskCoreAffinitySet(coast_event_handler_task_handle, 0x01);
+                add_alarm_in_ms(MOTOR_BURN_TIME, coast_event_callback, NULL, false);
+                vTaskDelete(launch_event_handler_task_handle);
+                launch_event_handler_task_handle = NULL;
+                xTaskResumeAll();
+            }
+        }
+    }
 }
 
-// LOGGING THREAD RELATED FUNCTIONS AND CALLBACKS
-//===============================================================================
+static void coast_event_handler_task(void* pvParameters) {
+    const TickType_t xInterruptFrequency = pdMS_TO_TICKS( MOTOR_BURN_TIME );
+    const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 );
+    uint32_t ulEventsToProcess;
+    while (1) {
+        /* Wait to receive a notification sent directly to this task from the
+        interrupt service routine. */
+        ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime );
+        if( ulEventsToProcess != 0 ) {
+            /* To get here at least one event must have occurred. Loop here
+            until all the pending events have been processed */
+            while( ulEventsToProcess > 0 ) {
+                vTaskSuspendAll();
+                rocket_state = COAST;
+                vTaskDelete(coast_event_handler_task_handle);
+                coast_event_handler_task_handle = NULL;
+                xTaskResumeAll();
+            }
+        }
+    }
+}
 
-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);
+static void end_event_handler_task(void* pvParameters) {
+    const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 30000 );
+    const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 );
 
+    uint32_t ulEventsToProcess;
     while (1) {
-        tight_loop_contents();
+        /* Wait to receive a notification sent directly to this task from the
+        interrupt service routine. */
+        ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime );
+        if( ulEventsToProcess != 0 ) {
+            /* To get here at least one event must have occurred. Loop here
+            until all the pending events have been processed */
+            while( ulEventsToProcess > 0 ) {
+                rocket_state = END;
+                vTaskDelete(end_event_handler_task_handle);
+                end_event_handler_task_handle = NULL;
+            }
+        }
     }
 }
 
-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;
+int64_t launch_event_callback(alarm_id_t id, void* user_data) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    // Defer ISR handling to separate handler within FreeRTOS context
+    vTaskNotifyGiveFromISR(launch_event_handler_task_handle, &xHigherPriorityTaskWoken );
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+    return 0;
+}
 
-    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];
+int64_t coast_event_callback(alarm_id_t id, void* user_data) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    // Defer ISR handling to separate handler within FreeRTOS context
+    vTaskNotifyGiveFromISR(coast_event_handler_task_handle, &xHigherPriorityTaskWoken );
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+    return 0;
 }
 
-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;
+int64_t end_event_callback(alarm_id_t id, void* user_data) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    // Defer ISR handling to separate handler within FreeRTOS context
+    vTaskNotifyGiveFromISR(end_event_handler_task_handle, &xHigherPriorityTaskWoken );
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+    return 0;
 }
 
-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;
+fix16_t calculate_drag_force(fix16_t deployment_percentage, fix16_t vertical_velocity) {
+    static const fix16_t p00 = F16(125.f);
+    static const fix16_t p10 = F16(-3.286f);
+    static const fix16_t p01 = F16(-1.803f);
+    static const fix16_t p20 = F16(0.01675f);
+    static const fix16_t p11 = F16(0.02687f);
+    static const fix16_t p02 = F16(0.008441f);
+
+    fix16_t term1 = fix16_mul(p10, deployment_percentage);
+    fix16_t term2 = fix16_mul(p01, vertical_velocity);
+    fix16_t term3 = fix16_mul(p20, fix16_sq(deployment_percentage));
+    fix16_t term4 = fix16_mul(fix16_mul(p11, vertical_velocity), deployment_percentage);
+    fix16_t term5 = fix16_mul(fix16_mul(p02, vertical_velocity), vertical_velocity);
+
+    fix16_t drag_force = fix16_add(p00, term1);
+            drag_force = fix16_add(drag_force, term2);
+            drag_force = fix16_add(drag_force, term3);
+            drag_force = fix16_add(drag_force, term4);
+            drag_force = fix16_add(drag_force, term5);
+
+    return drag_force;
 }
 
-bool heartbeat_callback(repeating_timer_t *rt) {
-    const bool sequence[] = {true, false, true, false, false};
-    const uint8_t sequence_length = 5;
+fix16_t predict_apogee(fix16_t altitude, fix16_t vertical_velocity, fix16_t drag_force) {
+    static const fix16_t gravity = F16(9.81f);
+    static const fix16_t mass = F16(21.8f);
+
+    fix16_t nal_log_internal = fix16_div(gravity, fix16_add(gravity, fix16_div(drag_force, mass)));
+    fix16_t nal_log_scale = fix16_mul(fix16_mul(fix16_div(vertical_velocity, fix16_mul(F16(2), drag_force)), vertical_velocity), mass);
+    fix16_t apogee_prediction = fix16_sub(altitude, fix16_mul(nal_log_scale, fix16_log(nal_log_internal)));
+    return apogee_prediction;
+}
 
-    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;
+fix16_t calculate_deployment_percentage(fix16_t drag_force, fix16_t vertical_velocity) {
+    static const fix16_t p00 = F16(79.05f);
+    static const fix16_t p10 = F16(1.057f);
+    static const fix16_t p01 = F16(-1.049f);
+    static const fix16_t p20 = F16(-7.296e-5f);
+    static const fix16_t p11 = F16(-0.003321f);
+    static const fix16_t p02 = F16(0.002322f);
+
+
+    fix16_t term1 = fix16_mul(p10, drag_force);
+    fix16_t term2 = fix16_mul(p01, vertical_velocity);
+    fix16_t term3 = fix16_mul(fix16_mul(p20, drag_force), drag_force);
+    fix16_t term4 = fix16_mul(fix16_mul(p11, drag_force), vertical_velocity);
+    fix16_t term5 = fix16_mul(fix16_mul(p02, vertical_velocity), vertical_velocity);
+
+    fix16_t deployment_percentage = fix16_add(p00, term1);
+            deployment_percentage = fix16_add(deployment_percentage, term2);
+            deployment_percentage = fix16_add(deployment_percentage, term3);
+            deployment_percentage = fix16_add(deployment_percentage, term4);
+            deployment_percentage = fix16_add(deployment_percentage, term5);
+            deployment_percentage = fix16_clamp(deployment_percentage, 0, fix16_from_int(100));
+    return deployment_percentage;
 }
 
+fix16_t calculate_desired_drag_force(fix16_t altitude, fix16_t vertical_velocity) {
+    static const fix16_t p00 = F16(-2.042e+01);
+    static const fix16_t p10 = F16(2.879e+01);
+    static const fix16_t p01 = F16(2.391e+02);
+    static const fix16_t p20 = F16(-1.265e+01);
+    static const fix16_t p11 = F16(-2.499e+02);
+    static const fix16_t p02 = F16(-1.063e+03);
+    static const fix16_t p30 = F16(1.774);
+    static const fix16_t p21 = F16(7.604e+01);
+    static const fix16_t p12 = F16(7.028e+02);
+    static const fix16_t p03 = F16(2.135e+03);
+    static const fix16_t p31 = F16(-6.349);
+    static const fix16_t p22 = F16(-1.049e+02);
+    static const fix16_t p13 = F16(-6.41e+02);
+    static const fix16_t p04 = F16(-1.604e+03);
+
+    fix16_t altitude_km = fix16_div(altitude, fix16_from_int(1000));
+    fix16_t vertical_velocity_km = fix16_div(vertical_velocity_km, fix16_from_int(1000));
+
+    fix16_t term01 = fix16_mul(p10, altitude_km);
+    fix16_t term02 = fix16_mul(p01, vertical_velocity_km);
+    fix16_t term03 = fix16_mul(fix16_mul(p20, altitude_km), altitude_km);
+    fix16_t term04 = fix16_mul(fix16_mul(p11, altitude_km), vertical_velocity_km);
+    fix16_t term05 = fix16_mul(fix16_mul(p02, vertical_velocity_km), vertical_velocity_km);
+    fix16_t term06 = fix16_mul(fix16_mul(fix16_mul(p30, altitude_km), altitude_km), altitude_km);
+    fix16_t term07 = fix16_mul(fix16_mul(fix16_mul(p21, altitude_km), altitude_km), vertical_velocity_km);
+    fix16_t term08 = fix16_mul(fix16_mul(fix16_mul(p12, altitude_km), vertical_velocity_km), vertical_velocity_km);
+    fix16_t term09 = fix16_mul(fix16_mul(fix16_mul(p03, vertical_velocity_km), vertical_velocity_km), vertical_velocity_km);
+    fix16_t term10 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p31, altitude_km), altitude_km), altitude_km), vertical_velocity_km);
+    fix16_t term11 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p22, altitude_km), altitude_km), vertical_velocity_km), vertical_velocity_km);
+    fix16_t term12 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p13, altitude_km), vertical_velocity_km), vertical_velocity_km), vertical_velocity_km);
+    fix16_t term13 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p04, vertical_velocity_km), vertical_velocity_km), vertical_velocity_km), vertical_velocity_km);
+
+    fix16_t desired_drag_force = fix16_add(p00, term01);
+            desired_drag_force = fix16_add(desired_drag_force, term02);
+            desired_drag_force = fix16_add(desired_drag_force, term03);
+            desired_drag_force = fix16_add(desired_drag_force, term04);
+            desired_drag_force = fix16_add(desired_drag_force, term05);
+            desired_drag_force = fix16_add(desired_drag_force, term06);
+            desired_drag_force = fix16_add(desired_drag_force, term07);
+            desired_drag_force = fix16_add(desired_drag_force, term08);
+            desired_drag_force = fix16_add(desired_drag_force, term09);
+            desired_drag_force = fix16_add(desired_drag_force, term10);
+            desired_drag_force = fix16_add(desired_drag_force, term11);
+            desired_drag_force = fix16_add(desired_drag_force, term12);
+            desired_drag_force = fix16_add(desired_drag_force, term13);
+
+            desired_drag_force = fix16_mul(desired_drag_force, fix16_from_int(1000));
+
+    return desired_drag_force;
+}
diff --git a/src/adxl375.cpp b/src/adxl375.cpp
new file mode 100644
index 0000000..5b18c42
--- /dev/null
+++ b/src/adxl375.cpp
@@ -0,0 +1,60 @@
+#include "adxl375.hpp"
+
+void ADXL375::initialize() {
+    //Configure power mode and ODR
+    buffer[0] = R_ADXL375_BW_RATE;
+    bw_rate.fields.LOW_POWER = B_ADXL375_BW_RATE_NORMAL_POWER_MODE;
+    bw_rate.fields.RATE = B_ADXL375_ODR_800_HZ;
+    buffer[1] = bw_rate.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+    //Configure data output format
+    buffer[0] = R_ADXL375_DATA_FORMAT;
+    i2c_write_blocking(i2c, addr, buffer, 1, true);
+    i2c_read_blocking(i2c, addr, buffer, 1, false);
+    //Some reserved fields are set to 1, read byte and *then* set desired bits
+    data_format.data = buffer[0];
+
+    buffer[0] = R_ADXL375_DATA_FORMAT;
+    data_format.fields.JUSTIFY = B_ADXL375_DATA_FORMAT_JUSTIFY_RIGHT;
+    data_format.fields.INT_INVERT = B_ADXL375_DATA_FORMAT_INTERRUPT_ACTIVE_HIGH;
+    buffer[1] = data_format.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+    //Set to measurement mode
+    buffer[0] = R_ADXL375_POWER_CTL;
+    power_ctl.fields.MEASURE = true;
+    buffer[1] = power_ctl.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+}
+
+void ADXL375::sample() {
+    //Read DATAX0 - DATAZ1 as a block
+    buffer[0] = R_ADXL375_DATAX0;
+    i2c_write_blocking(i2c, addr, buffer, 1, true);
+    i2c_read_blocking(i2c, addr, buffer, 6, false);
+
+    //Split buffer into individual fields
+    ax = ((int16_t) buffer[0]) | ((int16_t) buffer[1] << 8);
+    ay = ((int16_t) buffer[2]) | ((int16_t) buffer[3] << 8);
+    az = ((int16_t) buffer[4]) | ((int16_t) buffer[5] << 8);
+}
+
+#if (USE_FREERTOS == 1)
+void ADXL375::update_adxl375_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ADXL375_SAMPLE_RATE_HZ);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        taskENTER_CRITICAL();
+        ADXL375* adxl375 = (ADXL375 *) pvParameters;
+        adxl375->sample();
+        taskEXIT_CRITICAL();
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
+}
+#endif
diff --git a/src/altimeter.cpp b/src/altimeter.cpp
deleted file mode 100644
index 9c47ceb..0000000
--- a/src/altimeter.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-#include "altimeter.hpp"
-#include "hardware/gpio.h"
-
-altimeter::altimeter(i2c_inst_t* inst, uint8_t addr) {
-    this->inst = inst;
-    this->addr = addr;
-}
-
-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);
-}
-
-void altimeter::initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) {
-    this->initialize();
-
-    // Below configures the interrupt for the first transition from PAD to BOOST
-    // Initial Reading
-
-    sleep_ms(1000);
-
-    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
-}
-
-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;
-    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) ((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/heartbeat.cpp b/src/heartbeat.cpp
new file mode 100644
index 0000000..e627935
--- /dev/null
+++ b/src/heartbeat.cpp
@@ -0,0 +1,48 @@
+#include "heartbeat.hpp"
+
+static int led_pin;
+
+#if (USE_FREERTOS != 1)
+static repeating_timer_t heartbeat_timer;
+
+static bool heartbeat_callback(repeating_timer_t *rt) {
+    static volatile uint8_t led_counter = 0;
+    const bool sequence[] = {true, false, true, false, false};
+    const uint8_t sequence_length = 5;
+    bool led_status = sequence[led_counter];
+    gpio_put(led_pin, led_status);
+    led_counter++;
+    led_counter %= sequence_length;
+    return true;
+}
+#endif
+
+void heartbeat_initialize(int gpio) {
+    led_pin = gpio;
+    gpio_init(led_pin);
+    gpio_set_dir(led_pin, GPIO_OUT);
+    gpio_put(led_pin, 0);
+#if (USE_FREERTOS != 1)
+    add_repeating_timer_us(-1000000 / HEART_RATE_HZ,  &heartbeat_callback, NULL, &heartbeat_timer);
+#endif
+}
+
+
+#if (USE_FREERTOS == 1)
+void heartbeat_task( void *pvParameters ) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / HEART_RATE_HZ);
+    static volatile uint8_t led_counter = 0;
+    const bool sequence[] = {true, false, true, false, false};
+    const uint8_t sequence_length = 5;
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        bool led_status = sequence[led_counter];
+        gpio_put(PICO_DEFAULT_LED_PIN, led_status);
+        led_counter++;
+        led_counter %= sequence_length;
+    }
+}
+#endif
diff --git a/src/iim42653.cpp b/src/iim42653.cpp
new file mode 100644
index 0000000..d9b96d4
--- /dev/null
+++ b/src/iim42653.cpp
@@ -0,0 +1,108 @@
+#include "iim42653.hpp"
+
+int16_t IIM42653::sat_sub(int16_t a, int16_t b) {
+    int32_t result = (int32_t) a - (int32_t) b;
+    if (result < INT16_MIN) {
+        result = INT16_MIN;
+    }
+    if (result > INT16_MAX) {
+        result = INT16_MAX;
+    }
+    return (int16_t) result;
+};
+
+//Startup routine, initialize GPIO clock output
+void IIM42653::initialize() {
+    //Enable 40kHz clock output on GPIO 23
+    clock_gpio_init(MICRO_DEFAULT_CLK_OUTPUT, IIM42653_CLOCK_SOURCE_SYSTEM, IIM42653_CLOCK_DIVISOR);	
+    sleep_ms(50);	//Allow time for clock output to stabilize
+
+    //Configure Gyroscope FSR and ODR
+    buffer[0] = R_IIM42653_GYRO_CONFIG0;
+    gyro_config0.fields.GYRO_ODR = B_IIM42653_GYRO_CONFIG0_ODR_500HZ;
+    gyro_config0.fields.GYRO_UI_FS_SEL = B_IIM42653_GYRO_CONFIG0_FSR_4000DPS;
+    buffer[1] = gyro_config0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    
+    //Configure Accelerometer FSR and ODR
+    buffer[0] = R_IIM42653_ACCEL_CONFIG0;
+    accel_config0.fields.ACCEL_ODR = B_IIM42653_ACCEL_CONFIG0_ODR_500HZ;
+    accel_config0.fields.ACCEL_UI_FS_SEL = B_IIM42653_ACCEL_CONFIG0_FSR_32G;
+    buffer[1] = accel_config0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    
+    //Enable Gyroscope, Accelerometer, and Thermocouple
+    buffer[0] = R_IIM42653_PWR_MGMT0;
+    pwr_mgmt0.fields.ACCEL_MODE = B_IIM42653_PWR_MGMT0_ACCEL_MODE_LOW_NOISE;
+    pwr_mgmt0.fields.GYRO_MODE = B_IIM42653_PWR_MGMT0_GYRO_MODE_LOW_NOISE;
+    pwr_mgmt0.fields.TEMP_DIS = B_IIM42653_PWR_MGMT0_TEMP_ENABLE;
+    buffer[1] = pwr_mgmt0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    sleep_ms(10); //Datasheet instructs 200us wait after changing sensor power modes
+    
+    //Calibrate gyro bias once all sensors are initialized
+    calibrate_gyro();
+}
+
+//Read all 12 data registers at once and format them into their internal fields
+void IIM42653::sample() {
+    //Read ACCEL_DATA_X1_UI - GYRO_DATA_Z0_UI as a block
+    buffer[0] = R_IIM42653_ACCEL_DATA_X1_UI;
+    i2c_write_blocking(i2c, addr, buffer, 1, true);
+    i2c_read_blocking(i2c, addr, buffer, 12, false);
+
+    //Split buffer into individial fields
+    ax = ((int16_t)buffer[1]) | (((int16_t)buffer[0]) << 8);
+    ay = ((int16_t)buffer[3]) | (((int16_t)buffer[2]) << 8);
+    az = ((int16_t)buffer[5]) | (((int16_t)buffer[4]) << 8);
+    raw_gx = ((int16_t)buffer[7]) | (((int16_t)buffer[6]) << 8);
+    raw_gy = ((int16_t)buffer[9]) | (((int16_t)buffer[8]) << 8);
+    raw_gz = ((int16_t)buffer[11]) | (((int16_t)buffer[10]) << 8);
+}
+void IIM42653::apply_gyro_offset() {
+    gx = sat_sub(raw_gx, bias_gx);
+    gy = sat_sub(raw_gy, bias_gy);
+    gz = sat_sub(raw_gz, bias_gz);
+}
+
+//Request certain number of gyroscope readings, and set the gyro bias values to the averages of the readings
+void IIM42653::calibrate_gyro() {
+    //Initialize buffers
+    int64_t g_x = 0;
+    int64_t g_y = 0;
+    int64_t g_z = 0;
+
+    //Request configured number of samples and l
+    for (int64_t i = 0; i < n_gyro_bias_readings; i++) {
+        sample();
+        g_x += raw_gx;
+        g_y += raw_gy;
+        g_z += raw_gz;
+        sleep_ms(5);
+    }
+
+    //Set bias values to the average readings during the calibration period
+    bias_gx = g_x/n_gyro_bias_readings;
+    bias_gy = g_y/n_gyro_bias_readings;
+    bias_gz = g_z/n_gyro_bias_readings;
+}
+
+#if ( USE_FREERTOS == 1 )
+void IIM42653::update_iim42653_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / IIM42653_SAMPLE_RATE_HZ);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        taskENTER_CRITICAL();
+        IIM42653* iim42653 = (IIM42653*) pvParameters;
+        iim42653->sample();
+        iim42653->apply_gyro_offset();
+        taskEXIT_CRITICAL();
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
+}
+#endif
diff --git a/src/imu.cpp b/src/imu.cpp
deleted file mode 100644
index 4a81042..0000000
--- a/src/imu.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-#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/log_format.cpp b/src/log_format.cpp
new file mode 100644
index 0000000..5d9d275
--- /dev/null
+++ b/src/log_format.cpp
@@ -0,0 +1,66 @@
+#include "log_format.hpp"
+
+void print_log_entry(const uint8_t* entry) {
+    static bool first_call = true;
+
+    if (first_call) {
+        printf("time_us,state,temperature_chip_celsius,deployment_percent,pressure_mb,altitude,altitude_filt,velocity_filt,temperature_alt_celsius,mid_imu_ax,mid_imu_ay,mid_imu_az,mid_imu_gx,mid_imu_gy,mid_imu_gz,mag_x,mag_y,mag_z,high_g_x,high_g_y,high_g_z,drag_force,apogee_prediction,desired_drag_force\r\n");
+        first_call = false;
+    }
+
+    const log_entry_t* packet = reinterpret_cast<const log_entry_t *>(entry);
+    printf("%" PRIu64 ",", packet->time_us);
+    state_t state = (state_t) packet->state;
+    switch (state) {
+        case PAD:
+            printf("PAD");
+            break;
+        case BOOST:
+            printf("BOOST");
+            break;
+        case COAST:
+            printf("COAST");
+            break;
+        case APOGEE:
+            printf("APOGEE");
+            break;
+        case RECOVERY:
+            printf("RECOVERY");
+            break;
+        case END:
+            printf("END");
+            break;
+    }
+    printf(",");
+    const float conversionFactor = 3.3f / (1 << 12);
+    float tempC = 27.0f - (((float)(packet->temperature_chip) * conversionFactor) - 0.706f) / 0.001721f;
+    printf("%4.2f,", tempC);
+    printf("%d,", packet->deploy_percent);
+    printf("%4.2f,", ((float) packet->pressure) / PRESSURE_SCALE_F);
+    printf("%4.2f,", ((float) packet->altitude) / ALTITUDE_SCALE_F);
+    printf("%4.2f,", (fix16_to_float(packet->altitude_filt)));
+    printf("%4.2f,", (fix16_to_float(packet->velocity_filt)));
+    printf("%4.2f,", ((float) packet->temperature_alt) / TEMPERATURE_SCALE_F);
+
+    printf("%4.2f,", IIM42653::scale_accel(packet->ax));
+    printf("%4.2f,", IIM42653::scale_accel(packet->ay));
+    printf("%4.2f,", IIM42653::scale_accel(packet->az));
+    
+    printf("%4.2f,", IIM42653::scale_gyro(packet->gx));
+    printf("%4.2f,", IIM42653::scale_gyro(packet->gy));
+    printf("%4.2f,", IIM42653::scale_gyro(packet->gz));
+    
+    printf("%" PRIi16 ",", packet->mag_x);
+    printf("%" PRIi16 ",", packet->mag_y);
+    printf("%" PRIi16 ",", packet->mag_z);
+    
+    printf("%4.2f,", ADXL375::scale(packet->high_g_x));
+    printf("%4.2f,", ADXL375::scale(packet->high_g_y));
+    printf("%4.2f,", ADXL375::scale(packet->high_g_z));
+    printf("%4.2f,", (fix16_to_float(packet->drag_force)));
+    printf("%4.2f,", (fix16_to_float(packet->apogee_prediction)));
+    printf("%4.2f,", (fix16_to_float(packet->desired_drag_force)));
+    printf("\r\n");
+    stdio_flush();
+}
+
diff --git a/src/mmc5983ma.cpp b/src/mmc5983ma.cpp
new file mode 100644
index 0000000..45ef94a
--- /dev/null
+++ b/src/mmc5983ma.cpp
@@ -0,0 +1,158 @@
+#include "mmc5983ma.hpp"
+
+int16_t MMC5983MA::sat_sub(int16_t a, int16_t b) {
+    int32_t result = (int32_t) a - (int32_t) b;
+    if (result < INT16_MIN) {
+        result = INT16_MIN;
+    }
+    if (result > INT16_MAX) {
+        result = INT16_MAX;
+    }
+    return (int16_t) result;
+};
+
+void MMC5983MA::initialize() {
+    // Restart device prior to configuration
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
+    internal_ctl1.fields.RESTART = true;
+    buffer[1] = internal_ctl1.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    internal_ctl1.data = 0;
+
+    sleep_ms(100);
+
+    calibrate();
+
+    //Configure decimation filter bandwidth for 200 Hz
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
+    internal_ctl1.fields.BANDWIDTH = B_MMC5983MA_BANDWIDTH_200HZ;
+    buffer[1] = internal_ctl1.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+    //Configure and enable continuous measurement mode for 200 Hz
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL2;
+    internal_ctl2.fields.CONTINUOUS_MODE_ENABLE = true;
+    internal_ctl2.fields.CONTINUOUS_MODE_FREQ = B_MMC5983_CONTINUOUS_MODE_FREQ_200HZ;
+    internal_ctl2.fields.PERIODIC_SET_ENABLE = false;
+    internal_ctl2.fields.PERIODIC_SET_RATE = B_MMC5983_PERIODIC_SET_RATE_MEAS_TIMES_1000;
+    buffer[1] = internal_ctl2.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    printf("MMC5983MA Initialized!\n");
+}
+
+void MMC5983MA::sample() {
+    buffer[0] = R_MMC5983MA_XOUT0;
+    i2c_write_blocking(i2c, addr, buffer, 1, true);
+    i2c_read_blocking(i2c, addr, buffer, 6, false);
+
+    raw_x = (int16_t) ((((uint16_t) buffer[0] << 8) | buffer[1]) - 32768);
+    raw_y = (int16_t) ((((uint16_t) buffer[2] << 8) | buffer[3]) - 32768);
+    raw_z = (int16_t) ((((uint16_t) buffer[4] << 8) | buffer[5]) - 32768);
+}
+
+void MMC5983MA::apply_offset() {
+    ax = sat_sub(raw_x, offset_x);
+    ay = sat_sub(raw_y, offset_y);
+    az = sat_sub(raw_z, offset_z);
+}
+
+void MMC5983MA::calibrate() {
+    sleep_ms(100);
+    
+    printf("MMC5983MA Calibration | Set command");
+    // Take measurement after SET command completes
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+    internal_ctl0.fields.SET_CMD = true;
+    buffer[1] = internal_ctl0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    internal_ctl0.data = 0;
+
+    sleep_ms(100);
+    printf(" | Measurement Request");
+
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+    internal_ctl0.fields.TAKE_MAG_MEAS = true;
+    buffer[1] = internal_ctl0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    internal_ctl0.data = 0;
+    printf(" | Request sent, awaiting completion...\n");
+    
+    size_t counter = 0;
+    while (!dev_status.fields.MEAS_M_DONE) {
+        sleep_ms(1);
+        buffer[0] = R_MMC5983MA_DEV_STATUS;
+        i2c_write_blocking(i2c, addr, buffer, 1, true);
+        i2c_read_blocking(i2c, addr, buffer, 1, false);
+        dev_status.data = buffer[0];
+        printf("W | ");
+        counter++;
+        if (counter == 20) {
+            printf("\n");
+            counter = 0;
+        }
+    }
+    printf("\n\nMeasurement taken!");
+
+    sample();
+    printf(" | Sample taken!");
+
+    int16_t set_ax = raw_x, set_ay = raw_y, set_az = raw_z;
+
+    // Take measurement after RESET command completes
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+    internal_ctl0.fields.RESET_CMD = true;
+    buffer[1] = internal_ctl0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    internal_ctl0.data = 0;
+
+    sleep_ms(100);
+
+    buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+    internal_ctl0.fields.TAKE_MAG_MEAS = true;
+    buffer[1] = internal_ctl0.data;
+    i2c_write_blocking(i2c, addr, buffer, 2, false);
+    internal_ctl0.data = 0;
+    
+    while (!dev_status.fields.MEAS_M_DONE) {
+        sleep_ms(1);
+        buffer[0] = R_MMC5983MA_DEV_STATUS;
+        i2c_write_blocking(i2c, addr, buffer, 1, true);
+        i2c_read_blocking(i2c, addr, buffer, 1, false);
+        dev_status.data = buffer[0];
+    }
+
+    sample();
+
+    int16_t reset_ax = raw_x, reset_ay = raw_y, reset_az = raw_z;
+
+    sleep_ms(100);
+
+    // Average the two measurements taken and assign them as offsets
+    offset_x = (int16_t) (((int32_t) set_ax + (int32_t) reset_ax) / 2);
+    offset_y = (int16_t) (((int32_t) set_ay + (int32_t) reset_ay) / 2);
+    offset_z = (int16_t) (((int32_t) set_az + (int32_t) reset_az) / 2);
+
+#if ( DEBUG == 1 )
+    printf("MMC5983MA: Calibrated!\n\toffset_x: %" PRIi16 "\n\toffset_y: %" PRIi16 "\n\toffset_z: %" PRIi16 "\n", offset_x, offset_y, offset_z);
+#endif
+}
+
+#if (USE_FREERTOS == 1)
+void MMC5983MA::update_mmc5983ma_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MMC5983_SAMPLE_RATE_HZ);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        taskENTER_CRITICAL();
+        MMC5983MA* mag = (MMC5983MA *) pvParameters;
+        mag->sample();
+        mag->apply_offset();
+        taskEXIT_CRITICAL();
+        if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+            xLastWakeTime = xTaskGetTickCount();
+        }
+    }
+}
+#endif
diff --git a/src/ms5607.cpp b/src/ms5607.cpp
new file mode 100644
index 0000000..c093189
--- /dev/null
+++ b/src/ms5607.cpp
@@ -0,0 +1,208 @@
+#include "ms5607.hpp"
+
+static const int32_t altitude_table[] = {
+#include "altitude-pa.h"
+};
+
+#define ALT_SCALE (1 << ALT_SHIFT)
+#define ALT_MASK (ALT_SCALE - 1)
+
+void MS5607::initialize() {
+    sample_state = NOT_SAMPLING;
+
+    alarm_pool_init_default();
+
+    ms5607_cmd cmd;
+    cmd.data = RESET_COMMAND;
+
+    ms5607_write_cmd(&cmd);
+
+    sleep_ms(500);
+
+    cmd.data = 0;
+    cmd.fields.PROM = true;
+    cmd.fields.PROM2 = true;
+
+    cmd.fields.ADDR_OSR = PROM_CALIBRATION_COEFFICENT_1_ADDR;
+
+    for (uint8_t prom_addr = PROM_CALIBRATION_COEFFICENT_1_ADDR; prom_addr <= PROM_CALIBRATION_COEFFICENT_6_ADDR; prom_addr++) {
+        sleep_ms(100);
+        cmd.fields.ADDR_OSR = prom_addr;
+        ms5607_write_cmd(&cmd);
+        i2c_read_blocking(i2c, addr, buffer, 2, false);
+
+        prom[prom_addr - 1] = static_cast<uint16_t>((((uint16_t) buffer[0]) << 8) | ((uint16_t) buffer[1]));
+    }
+}
+
+void MS5607::ms5607_write_cmd(ms5607_cmd* cmd) {
+    i2c_write_blocking(i2c, addr, (uint8_t *) cmd, 1, false);
+}
+
+void MS5607::ms5607_start_sample() {
+    if (sample_state == NOT_SAMPLING) {
+        sample_state = PRESSURE_CONVERT;
+        ms5607_sample();
+    }
+}
+
+#if (USE_FREERTOS == 1)
+void MS5607::ms5607_sample_handler(void* pvParameters) {
+    /* xMaxExpectedBlockTime is set to be a little longer than the maximum
+    expected time between events. */
+    const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 1000 / (MS5607_SAMPLE_RATE_HZ * 2) );
+    const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 1 );
+    uint32_t ulEventsToProcess;
+    while (1) {
+        /* Wait to receive a notification sent directly to this task from the
+        interrupt service routine. */
+        ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime );
+        if( ulEventsToProcess != 0 ) {
+            /* To get here at least one event must have occurred. Loop here
+            until all the pending events have been processed */
+            while( ulEventsToProcess > 0 ) {
+                MS5607* alt = (MS5607 *) pvParameters;
+                taskENTER_CRITICAL();
+                alt->ms5607_sample();
+                taskEXIT_CRITICAL();
+                ulEventsToProcess--;
+            }
+        }
+    }
+
+}
+
+void MS5607::update_ms5607_task(void* pvParameters) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MS5607_SAMPLE_RATE_HZ);
+
+    xLastWakeTime = xTaskGetTickCount();
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        MS5607* alt = (MS5607 *) pvParameters;
+        taskENTER_CRITICAL();
+        alt->ms5607_start_sample();
+        taskEXIT_CRITICAL();
+    }
+}
+#endif
+
+int64_t MS5607::ms5607_sample_callback(alarm_id_t id, void* user_data) {
+#if ( USE_FREERTOS == 1 )
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    MS5607* alt = (MS5607 *) user_data;
+    // Defer ISR handling to separate handler within FreeRTOS context
+    vTaskNotifyGiveFromISR(alt->sample_handler_task_handle, &xHigherPriorityTaskWoken );
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+#else
+    MS5607* alt = (MS5607 *) user_data;
+    alt->ms5607_sample();
+#endif
+    return 0;
+}
+
+void MS5607::ms5607_sample() {
+    ms5607_cmd cmd = {.data = 0};
+
+    switch (sample_state) {
+        case NOT_SAMPLING:
+            break;
+        case PRESSURE_CONVERT: {
+            cmd.fields.CONVERT = 1;
+            cmd.fields.TYPE = TYPE_UNCOMPENSATED_PRESSURE;
+            cmd.fields.ADDR_OSR = OSR_CONVERT_256;
+
+            ms5607_write_cmd(&cmd);
+
+            add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true);
+
+            sample_state = TEMPERATURE_CONVERT;
+            break;
+        };
+        case TEMPERATURE_CONVERT: {
+            cmd.data = ADC_READ_COMMAND;
+            ms5607_write_cmd(&cmd);
+            i2c_read_blocking(i2c, addr, buffer, 3, false);
+            uncompensated_pressure = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]);
+
+            cmd.fields.CONVERT = 1;
+            cmd.fields.TYPE = TYPE_UNCOMPENSATED_TEMPERATURE;
+            cmd.fields.ADDR_OSR = OSR_CONVERT_256;
+
+            ms5607_write_cmd(&cmd);
+
+            add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true);
+
+            sample_state = COMPENSATE;
+            break;
+        };
+        case COMPENSATE: {
+            cmd.data = ADC_READ_COMMAND;
+            ms5607_write_cmd(&cmd);
+            i2c_read_blocking(i2c, addr, buffer, 3, false);
+            uncompensated_temperature = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]);
+            ms5607_compensate();
+            altitude = pressure_to_altitude(pressure);
+
+
+            sample_state = NOT_SAMPLING;
+            if (threshold_callback != NULL) {
+                if (positive_crossing) {
+                    if (altitude >= threshold_altitude) {
+                        add_alarm_in_ms(1, threshold_callback, NULL, true);
+                    }
+                } else {
+                    if (altitude <= threshold_altitude) {
+                        add_alarm_in_ms(1, threshold_callback, NULL, true);
+                    }
+                }
+            }
+            break;
+        };
+    };
+
+}
+
+void MS5607::ms5607_compensate() {
+    int32_t dT = uncompensated_temperature - (((uint32_t) prom[4]) << 8);
+    temperature = 2000 + ( ( ( (int64_t) dT) * ( (int64_t) prom[5]) ) >> 23);
+    int64_t OFF = ( ( (int64_t) prom[1]) << 17) + ( ( ((int64_t) prom[3]) * ( (int64_t) dT)) >> 6);
+    int64_t SENS = ( ( (int64_t) prom[0]) << 16) + (( ( (int64_t) prom[2]) * ((int64_t) dT)) >> 7);
+    pressure = (int32_t) ((((((int64_t) uncompensated_pressure) * SENS) >> 21) - OFF) >> 15);
+
+}
+
+int32_t MS5607::pressure_to_altitude(int32_t pressure) {
+	uint16_t o;
+	int16_t	part;
+	int32_t low, high;
+
+	if (pressure < 0) {
+		pressure = 0;
+    }
+
+	if (pressure > 120000) {
+		pressure = 120000;
+    }
+
+	o = (uint16_t) (pressure >> ALT_SHIFT);
+	part = pressure & ALT_MASK;
+
+	low = (int32_t) altitude_table[o] * (ALT_SCALE - part);
+	high = (int32_t) altitude_table[o + 1] * part;
+	return ((low + high + (ALT_SCALE >> 1)) >> ALT_SHIFT);
+}
+
+
+void MS5607::set_threshold_altitude(int32_t threshold_altitude, alarm_callback_t callback) {
+    this->threshold_altitude = threshold_altitude;
+
+    positive_crossing = (threshold_altitude > altitude);
+
+    threshold_callback = callback;
+}
+
+void MS5607::clear_threshold_altitude() {
+    this->threshold_callback = NULL;
+}
diff --git a/src/pwm.cpp b/src/pwm.cpp
index 3a78d23..267765b 100644
--- a/src/pwm.cpp
+++ b/src/pwm.cpp
@@ -2,9 +2,9 @@
 
 void PWM::init() {
     // Tell GPIO 0 they are allocated to the PWM
-    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
+    gpio_set_function(MICRO_DEFAULT_PWM, GPIO_FUNC_PWM);
     // Find out which PWM slice is connected to GPIO 0 (it's slice 0)
-    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
+    uint slice_num = pwm_gpio_to_slice_num(MICRO_DEFAULT_PWM);
 
     // Configure PWM slice and set it running
     pwm_config cfg = pwm_get_default_config();
@@ -24,7 +24,7 @@ void PWM::set_duty_cycle(int duty_cycle_percent) {
     uint32_t raw_value = WRAP_VALUE * (duty_cycle_percent / 100.0);
 
     // Set the duty cycle
-    pwm_set_gpio_level(SERVO_PIN, raw_value);
+    pwm_set_gpio_level(MICRO_DEFAULT_PWM, raw_value);
 }
 
 void PWM::set_servo_percent(int percent) {
diff --git a/src/serial.cpp b/src/serial.cpp
new file mode 100644
index 0000000..9dfd5d9
--- /dev/null
+++ b/src/serial.cpp
@@ -0,0 +1,134 @@
+#include "serial.hpp"
+#include "portable.h"
+#include "portmacro.h"
+#include <pico/multicore.h>
+#include <pico/stdio.h>
+
+void serial_task( void *pvParameters ) {
+    char *str = (char *) malloc(1024);
+    size_t len = 1024;
+
+    
+    printf("Welcome! :3\n");
+
+    while (1) {
+        printf("# ");
+        stdio_flush();
+
+        size_t n = input_line(str,len);
+        printf("\n");
+        if (n > 0) {
+            for (int i = 0; i < (NUM_BASE_CMDS + num_user_cmds); i++) {
+                if ((i < NUM_BASE_CMDS) && n >= base_commands[i].len && strncmp(str, base_commands[i].name, base_commands[i].len) == 0) {
+                    base_commands[i].function();
+                    break;
+                }
+
+                if ((i >= NUM_BASE_CMDS && ((i - NUM_BASE_CMDS) < num_user_cmds)) && n >= user_commands[i - NUM_BASE_CMDS].len && strncmp(str, user_commands[i - NUM_BASE_CMDS].name, user_commands[i - NUM_BASE_CMDS].len) == 0) {
+                    user_commands[i - NUM_BASE_CMDS].function();
+                    break;
+                }
+                if (i == (NUM_BASE_CMDS + num_user_cmds - 1)) {
+                    printf("Invalid command! Please try again or use 'help' to see the available commands.\n");
+                }
+            }
+        }
+    }
+}
+
+int input_line(char *buffer, size_t len) {
+    size_t n = 0;
+    int c;
+    for (n = 0; n < len - 1; n++) {
+
+        c = getchar_timeout_us(0);
+        switch (c) {
+            case 127: /* fall through to below */
+            case '\b': /* backspace character received */
+                if (n > 0)
+                    n--;
+                buffer[n] = 0;
+                stdio_putchar('\b'); /* output backspace character */
+                stdio_putchar(' ');
+                stdio_putchar('\b');
+                n--; /* set up next iteration to deal with preceding char location */
+                break;
+            case '\n': /* fall through to \r */
+            case '\r':
+                buffer[n] = 0;
+                return n;
+            default:
+                if (c != PICO_ERROR_TIMEOUT && c < 256) {
+                    stdio_putchar(c);
+                    buffer[n] = c;
+                } else {
+                    n--;
+                }
+            break;
+        }
+    }
+    buffer[len - 1] = 0;
+    return 0; // Filled up buffer without reading a linebreak
+}
+
+void info_cmd_func() {
+    extern const char* executeable_name;
+    printf("%s", logo);
+    printf("#####################################################################################\n");
+    printf("#                          \e[0;31mRocketry \e[0;37mat \e[0;33mVirginia Tech\e[0;37m                                #\n");
+    printf("#                         Executeable: %s                       #\n", executeable_name);
+    printf("#####################################################################################\n\n");
+}
+
+void help_cmd_func() {
+    printf("Commands: \n");
+    for (int i = 0; i < NUM_BASE_CMDS; i++) {
+        printf("\t%s\n", base_commands[i].name);
+    }
+
+    for (int i = 0; i < num_user_cmds; i++) {
+        printf("\t%s\n", user_commands[i].name);
+    }
+}
+
+void clear_cmd_func() {
+    printf("%c%c%c%c",0x1B,0x5B,0x32,0x4A);
+}
+
+void top_cmd_func() {
+#if (configGENERATE_RUN_TIME_STATS == 1)
+    UBaseType_t num_tasks = uxTaskGetNumberOfTasks();
+    char* buffer = (char *) malloc( 40 * num_tasks );
+    vTaskGetRunTimeStats(buffer);
+    printf("%s", buffer);
+    free(buffer);
+#endif
+#if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
+    HeapStats_t heap_stats;
+    vPortGetHeapStats(&heap_stats);
+    printf("\n\tAvailable Heap Space In Bytes:\t%d\nSize Of Largest Free Block In Bytes:\t%d\nNumber Of Successful Allocations:\t%d\n", heap_stats.xAvailableHeapSpaceInBytes, heap_stats.xSizeOfLargestFreeBlockInBytes, heap_stats.xNumberOfSuccessfulAllocations);
+#endif
+}
+
+static void reset_cmd_task(void * unused_arg) {
+    reset_cmd_func();
+}
+
+void reset_cmd_func() {
+#define AIRCR_Register (*((volatile uint32_t*)(PPB_BASE + 0x0ED0C)))
+    if (get_core_num() == 0) {
+        vTaskSuspendAll();
+        multicore_reset_core1();
+        printf("\nOn core zero! Going dark!\n");
+        stdio_flush();
+        AIRCR_Register = 0x5FA0004;
+    } else {
+        printf("\nOn core one! Tasking core zero with the reset!\n");
+        stdio_flush();
+        TaskHandle_t reset_task = NULL;
+        vTaskSuspendAll();
+        xTaskCreate(reset_cmd_task, "reset", 256, NULL, (configMAX_PRIORITIES - 1), &reset_task);
+        vTaskCoreAffinitySet( reset_task, 0x01 );
+        xTaskResumeAll();
+    }
+}
diff --git a/src/spi_flash.c b/src/spi_flash.c
deleted file mode 100644
index 32babc5..0000000
--- a/src/spi_flash.c
+++ /dev/null
@@ -1,267 +0,0 @@
-/**
- * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-// Example of reading/writing an external serial flash using the PL022 SPI interface
-
-#include "spi_flash.h"
-
-static inline void cs_select(uint cs_pin) {
-    asm volatile("nop \n nop \n nop"); // FIXME
-    gpio_put(cs_pin, 0);
-    asm volatile("nop \n nop \n nop"); // FIXME
-}
-
-static inline void cs_deselect(uint cs_pin) {
-    asm volatile("nop \n nop \n nop"); // FIXME
-    gpio_put(cs_pin, 1);
-    asm volatile("nop \n nop \n nop"); // FIXME
-}
-
-void __not_in_flash_func(flash_read)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* dest, size_t len) {
-    cs_select(cs_pin);
-    uint8_t cmdbuf[4] = {
-            FLASH_CMD_READ,
-            addr >> 16,
-            addr >> 8,
-            addr
-    };
-    spi_write_blocking(spi, cmdbuf, 4);
-    spi_read_blocking(spi, 0, dest, len);
-    cs_deselect(cs_pin);
-}
-
-void __not_in_flash_func(flash_write_enable)(spi_inst_t *spi, uint cs_pin) {
-    cs_select(cs_pin);
-    uint8_t cmd = FLASH_CMD_WRITE_EN;
-    spi_write_blocking(spi, &cmd, 1);
-    cs_deselect(cs_pin);
-}
-
-void __not_in_flash_func(flash_wait_done)(spi_inst_t *spi, uint cs_pin) {
-    uint8_t status;
-    do {
-        cs_select(cs_pin);
-        uint8_t buf[2] = {FLASH_CMD_STATUS, 0};
-        spi_write_read_blocking(spi, buf, buf, 2);
-        cs_deselect(cs_pin);
-        status = buf[1];
-    } while (status & FLASH_STATUS_BUSY_MASK);
-}
-
-void __not_in_flash_func(flash_sector_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
-    uint8_t cmdbuf[4] = {
-            FLASH_CMD_SECTOR_ERASE,
-            addr >> 16,
-            addr >> 8,
-            addr
-    };
-    flash_write_enable(spi, cs_pin);
-    cs_select(cs_pin);
-    spi_write_blocking(spi, cmdbuf, 4);
-    cs_deselect(cs_pin);
-    flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_block_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
-    uint8_t cmdbuf[4] = {
-            FLASH_CMD_BLOCK_ERASE,
-            addr >> 16,
-            addr >> 8,
-            addr
-    };
-    flash_write_enable(spi, cs_pin);
-    cs_select(cs_pin);
-    spi_write_blocking(spi, cmdbuf, 4);
-    cs_deselect(cs_pin);
-    flash_wait_done(spi, cs_pin);
-}
-
-
-void __not_in_flash_func(flash_erase)(spi_inst_t *spi, uint cs_pin) {
-    uint8_t cmdbuf[1] = {
-            FLASH_CMD_CHIP_ERASE
-    };
-    flash_write_enable(spi, cs_pin);
-    cs_select(cs_pin);
-    spi_write_blocking(spi, cmdbuf, 1);
-    cs_deselect(cs_pin);
-    flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_page_program)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src) {
-    uint8_t cmdbuf[4] = {
-            FLASH_CMD_PAGE_PROGRAM,
-            addr >> 16,
-            addr >> 8,
-            addr
-    };
-    flash_write_enable(spi, cs_pin);
-    cs_select(cs_pin);
-    spi_write_blocking(spi, cmdbuf, 4);
-    spi_write_blocking(spi, src, FLASH_PAGE_SIZE);
-    cs_deselect(cs_pin);
-    flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_write)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src, size_t size) {
-    uint8_t cmdbuf[4] = {
-            FLASH_CMD_PAGE_PROGRAM,
-            addr >> 16,
-            addr >> 8,
-            addr
-    };
-    flash_write_enable(spi, cs_pin);
-    cs_select(cs_pin);
-    spi_write_blocking(spi, cmdbuf, 4);
-    spi_write_blocking(spi, src, size);
-    cs_deselect(cs_pin);
-    flash_wait_done(spi, cs_pin);
-}
-
-void write_entry(uint8_t* data_entry) {
-    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;
-        }
-    }
-    flash_write(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, data_entry, PACKET_SIZE);
-    base_addr += PACKET_SIZE;
-}
-
-
-#ifdef FLASH_TEST
-    #include "pico/multicore.h"
-    #include <stdint.h>
-    #include <inttypes.h>
-    #include <stdio.h>
-
-
-    void printbuf(uint8_t buf[FLASH_PAGE_SIZE]) {
-        for (int i = 0; i < FLASH_PAGE_SIZE; ++i) {
-            if (i % 16 == 15)
-                printf("%02x\n", buf[i]);
-            else
-                printf("%02x ", buf[i]);
-        }
-    }
-
-
-
-    void core1_entry() {
-        printf("SPI flash example\n");
-
-        // 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);
-
-        // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN);
-        // flash_sector_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0);
-        printf("Before program:\n");
-        flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
-        printbuf(page_buffer);
-
-        uint8_t entry[PACKET_SIZE];
-
-        printf("Written Data:\n");
-        printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\n");
-        for (uint16_t i = 0; i < 500; i++) {
-            absolute_time_t now = get_absolute_time();
-            uint64_t now_us= to_us_since_boot(now);
-            float altitude = 10.0f * i;
-            float velocity = 5.0f * i;
-            uint8_t deploy_percent = (i*1000) / 200;
-            printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\tDAWSYN_SCHRAIB\n", now_us, (uint8_t)(i), deploy_percent, altitude, velocity);
-            uint32_t alt_bits = *((uint32_t *)&altitude);
-            uint32_t vel_bits = *((uint32_t *)&velocity);
-            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;
-            entry[8] = i;
-            entry[9] = deploy_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] = 'D';
-            entry[19] = 'A';
-            entry[20] = 'W';
-            entry[21] = 'S';
-            entry[22] = 'Y';
-            entry[23] = 'N';
-            entry[24] = '_';
-            entry[25] = 'S';
-            entry[26] = 'C';
-            entry[27] = 'H';
-            entry[28] = 'R';
-            entry[29] = 'A';
-            entry[30] = 'I';
-            entry[31] = 'B';
-            write_entry(entry);
-        }
-
-        printf("After program:\n");
-        flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
-        printbuf(page_buffer);
-
-        printf("\nRead Data:\n");
-        printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\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];
-            uint8_t deploy_percent = entry[9];
-
-            uint32_t alt_bits = (entry[10] << 24) | (entry[11] << 16) | (entry[12] << 8) | (entry[13]);
-            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);
-            printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\t%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n", \
-                    now_us, state, deploy_percent, altitude, velocity, \
-                    entry[18],entry[19],entry[20],entry[21],entry[22],entry[23],entry[24],entry[25],entry[26],entry[27],entry[28],entry[29],entry[30],entry[31]);
-        }
-
-    }
-
-    int main() {
-        // Enable UART so we can print status output
-        stdio_init_all();
-        getchar();
-
-        multicore_launch_core1(core1_entry);
-
-        while (1) {
-            tight_loop_contents();
-        }
-
-    }
-#endif