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

1 files changed, 538 insertions, 0 deletions

diff --git a/tools/read_flash.cpp b/tools/read_flash.cpp
new file mode 100644
index 0000000..b3305aa
--- /dev/null
+++ b/tools/read_flash.cpp
@@ -0,0 +1,538 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <cmath>    //TODO: Reimplement math properly using Eigen functions
+#include <sstream>  //TODO: This is for debug purposes, remove when implemented
+#include <Eigen/Dense>
+using namespace Eigen;  //TODO: Limit scope to necessary components once implemented
+
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "hardware/adc.h"
+#include <hardware/timer.h>
+#include "pico/rand.h"
+#include "pico/stdlib.h"
+#include "pico/stdio.h"
+#include "pico/multicore.h"
+#include "pico/time.h"
+#include <pico/error.h>
+#include <pico/types.h>
+
+/* Kernel includes. */
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "portmacro.h"
+#include "projdefs.h"
+#include "serial.hpp"
+#include "task.h"
+#include "semphr.h"
+
+#include "heartbeat.hpp"
+#include "adxl375.hpp"
+#include "ms5607.hpp"
+#include "iim42653.hpp"
+#include "mmc5983ma.hpp"
+#include "serial.hpp"
+#include "pico_logger.h"
+#include "log_format.hpp"
+
+/* Priorities at which the tasks are created. */
+#define EVENT_HANDLER_PRIORITY      ( tskIDLE_PRIORITY + 4 )
+#define SENSOR_SAMPLE_PRIORITY      ( tskIDLE_PRIORITY + 3 )
+#define	HEARTBEAT_TASK_PRIORITY		( tskIDLE_PRIORITY + 2 )
+#define LOGGING_PRIORITY            ( tskIDLE_PRIORITY + 2 )
+#define	SERIAL_TASK_PRIORITY		( tskIDLE_PRIORITY + 1 )
+
+#define SENSOR_SAMPLE_RATE_HZ 500
+#define ORIENTATION_ESTIMATION_RATE_HZ 5
+
+#define MAX_SCL 400000
+
+static void populate_log_entry(log_entry_t * log_entry);
+static void sample_cmd_func();
+static void debug_cmd_func();
+static void circular_cmd_func();
+static void read_cmd_func();
+static void write_cmd_func();
+static void erase_cmd_func();
+static void orient_cmd_func();
+static void logging_task(void * unused_arg);
+
+void vApplicationTickHook(void) { /* optional */ }
+void vApplicationMallocFailedHook(void) { /* optional */ }
+void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { for( ;; ); }
+
+const char* executeable_name = "read_flash.uf2";
+const size_t num_user_cmds = 7;
+const command_t user_commands[] = { {.name = "sample",
+                                     .len = 6,
+                                     .function = &sample_cmd_func},
+                                    {.name = "debug",
+                                     .len = 5,
+                                     .function = &debug_cmd_func},
+                                    {.name = "circular",
+                                     .len = 8,
+                                     .function = &circular_cmd_func},
+                                    {.name = "read",
+                                     .len = 4,
+                                     .function = &read_cmd_func},
+                                    {.name = "write",
+                                     .len = 5,
+                                     .function = &write_cmd_func},
+                                    {.name = "erase",
+                                     .len = 5,
+                                     .function = &erase_cmd_func},
+                                    {.name = "orient",
+                                     .len = 6,
+                                     .function = &orient_cmd_func} };
+
+volatile bool serial_data_output = false;
+volatile bool use_circular_buffer = false;
+
+MS5607 alt(i2c_default);
+ADXL375 adxl375(i2c_default);
+IIM42653 iim42653(i2c_default);
+MMC5983MA mmc5983ma(i2c_default);
+
+Logger logger(PACKET_SIZE, LOG_BASE_ADDR, print_log_entry);
+
+volatile TaskHandle_t logging_handle = NULL;
+
+int main() {
+    stdio_init_all();
+
+    adc_init();
+    adc_set_temp_sensor_enabled(true);
+
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+    gpio_put(PICO_DEFAULT_LED_PIN, 0);
+
+    i2c_init(i2c_default, MAX_SCL);
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+    sleep_ms(2500);
+
+    info_cmd_func();
+    stdio_flush();
+
+    logger.initialize(true);
+
+    alt.initialize();
+    sleep_ms(500);
+    adxl375.initialize();
+    sleep_ms(500);
+    iim42653.initialize();
+    sleep_ms(500);
+    mmc5983ma.initialize();
+    sleep_ms(500);
+
+    xTaskCreate(heartbeat_task, "heartbeat", 256, NULL, HEARTBEAT_TASK_PRIORITY, NULL);
+    xTaskCreate(serial_task, "serial", 8192, NULL, SERIAL_TASK_PRIORITY, NULL);
+
+    vTaskStartScheduler();
+
+    while (1) {
+        tight_loop_contents();
+    }
+}
+
+static void logging_task(void * unused_arg) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / 10);
+
+    xLastWakeTime = xTaskGetTickCount();
+    printf("Time,Pressure,Altitude,Temperature,ax,ay,az,ax,ay,az,gx,gy,gz,ax,ay,az\n");
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+//        printf("%" PRIu64 ",", time_us_64());
+//        printf("%4.2f,", ((float) alt.get_pressure()) / PRESSURE_SCALE_F);
+//        printf("%4.2f,", ((float) alt.get_altitude()) / ALTITUDE_SCALE_F);
+//        printf("%4.2f,", ((float) alt.get_temperature()) / TEMPERATURE_SCALE_F);
+//        printf("%4.2f,", adxl375.scale(adxl375.get_ax()));
+//        printf("%4.2f,", adxl375.scale(adxl375.get_ay()));
+//        printf("%4.2f,", adxl375.scale(adxl375.get_az()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_ax()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_ay()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_az()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gx()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gy()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gz()));
+//        printf("%" PRIi16 ",", mmc5983ma.get_ax());
+//        printf("%" PRIi16 ",", mmc5983ma.get_ay());
+//        printf("%" PRIi16 ",", mmc5983ma.get_az());
+//        printf("\r\n");
+        log_entry_t log_entry;
+        populate_log_entry(&log_entry);
+        printf("\nWriting the following entry!\n");
+        print_log_entry(reinterpret_cast<const uint8_t *>(&log_entry));
+        if (use_circular_buffer) {
+            logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(&log_entry));
+        } else {
+            logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true);
+        }
+        stdio_flush();
+    }
+
+}
+
+//TODO: This task is getting increasingly complex and involved, it may be worth migrating to a class of its own
+static void pose_estimation_task(void * unused_arg) {
+    printf("--POSE: INITIALIZING DATA MEMBERS\n");
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ORIENTATION_ESTIMATION_RATE_HZ);
+    xLastWakeTime = xTaskGetTickCount();
+    int orient_count = 0;
+
+    Quaternionf q_k(1, 0, 0, 0);     //Initialize to straight upright
+    Quaternionf q_k_est = q_k;
+    Matrix4f P_k = Matrix4f::Identity()*0.01f;  //TODO: Tune this initialization value
+    Matrix4f P_k_est = P_k;
+    
+    //TODO: Store covariance values somewhere reasonable instead of hardcoding them
+    Matrix3f gyro_covariance = Matrix3f::Identity()*0.05f*M_PI/180.0f;   //0.05deg/s RMSE @ 100HZ Bandwidth
+    Matrix<float, 6, 6> accel_mag_covariance;
+    accel_mag_covariance << 0.00065f, 0, 0, 0, 0, 0,
+                            0, 0.00065f, 0, 0, 0, 0,
+                            0, 0, 0.00070f, 0, 0, 0,
+                            0, 0, 0, 0.00120f, 0, 0,
+                            0, 0, 0, 0, 0.00120f, 0,
+                            0, 0, 0, 0, 0, 0.00120f;
+    //accel_mag_covariance *= 3;
+
+    //Stored intermediate equation matrices
+    Matrix4f gyro_skew;                     //Skew-symmetric matrix equivalent of gyroscope output
+    Matrix4f I_4 = Matrix4f::Identity();    //4x4 Identity matrix, used for F_k calculation
+    
+    Matrix4f F_k;                           //State Transition Matrix
+    Matrix4f H_k;                           //Observation Matrix
+    Matrix4f K_k;                           //Kalman Gain Matrix
+    Matrix<float, 4, 3> J_process_k;        //Jacobian for process (gyro) covariance transformation
+    Matrix<float, 4, 6> J_observation_k;    //Jacobian for observation (accel/mag) covariance transformation
+    Matrix4f process_noise_transformed;     //Matrix of transformed process noise
+    Matrix4f observation_noise_transformed; //Matrix of transformed observation noise
+    Matrix<float, 4, 1> z_k;                //Measurement vector
+    Matrix<float, 4, 1> z_k_est;            //Estimated measurement vector, used for Kalman filter calculation
+    
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        
+        //printf("-------- BEGINNING UPDATE COMPUTATION --------\n");
+        //Get all required sensor values and compute intermediate values
+        float imu_ax = -IIM42653::scale_accel(iim42653.get_ax());
+        float imu_ay = -IIM42653::scale_accel(iim42653.get_ay());
+        float imu_az = IIM42653::scale_accel(iim42653.get_az());
+        float imu_gx = -IIM42653::scale_gyro(iim42653.get_gx())/180.0f*M_PI;
+        float imu_gy = -IIM42653::scale_gyro(iim42653.get_gy())/180.0f*M_PI;
+        float imu_gz = IIM42653::scale_gyro(iim42653.get_gz())/180.0f*M_PI;
+        float mag_x = -MMC5983MA::scale_mag(mmc5983ma.get_ay());   //TODO: Not convinced mag axes are correct
+        float mag_y = MMC5983MA::scale_mag(mmc5983ma.get_ax());
+        float mag_z = -MMC5983MA::scale_mag(mmc5983ma.get_az());
+
+        //Normalize accelerometer and magnetometer measurements
+        float imu_a_mag = std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_ay, 2) + std::pow(imu_az, 2));
+        float mag_mag = std::sqrt(std::pow(mag_x, 2) + std::pow(mag_y, 2) + std::pow(mag_z, 2));
+        imu_ax /= imu_a_mag;
+        imu_ay /= imu_a_mag;
+        imu_az /= imu_a_mag;
+        mag_x /= mag_mag;
+        mag_y /= mag_mag;
+        mag_z /= mag_mag;
+
+        printf("--- [I] INITIALIZATION | IMU Accel direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_ax, imu_ay, imu_az);
+        printf("--- [I] INITIALIZATION | IMU Gyro direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_gx, imu_gy, imu_gz);
+        printf("--- [I] INITIALIZATION | Magnetometer direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", mag_x, mag_y, mag_z);
+
+        float mag_D = imu_ax*mag_x + imu_ay*mag_y + imu_az*mag_z;
+        float mag_N = std::sqrt(1.0f - std::pow(mag_D, 2));
+        printf("--- [I] INITIALIZATION | Magnetometer values (mag_mag, mag_D, mag_N): [%2.4f, %2.4f, %2.4f]\n", mag_mag, mag_D, mag_N);
+        
+        //UPDATE | Calculate Kalman gain
+        //TODO: This Jacobian calculation is FUGLY, and very computationally intensive
+        //   Separate out common math, vectorize, and put it in a function somewhere?
+        float pd_mD_ax = mag_x/(2*mag_D);
+        float pd_mD_ay = mag_y/(2*mag_D);
+        float pd_mD_az = mag_z/(2*mag_D);
+        float pd_mD_mx = imu_ax/(2*mag_D);
+        float pd_mD_my = imu_ay/(2*mag_D);
+        float pd_mD_mz = imu_az/(2*mag_D);
+        float pd_mN_ax = -mag_D/mag_N*pd_mD_ax;
+        float pd_mN_ay = -mag_D/mag_N*pd_mD_ay;
+        float pd_mN_az = -mag_D/mag_N*pd_mD_az;
+        float pd_mN_mx = -mag_D/mag_N*pd_mD_mx;
+        float pd_mN_my = -mag_D/mag_N*pd_mD_my;
+        float pd_mN_mz = -mag_D/mag_N*pd_mD_mz;
+        float common_num = (imu_ay*mag_z - imu_az*mag_y);
+        float common_denom = std::pow(mag_N, 2);
+
+        float J_o_4_1 = (-pd_mN_ax*common_num)/common_denom;
+        float J_o_4_2 = (mag_z*mag_N - pd_mN_ay*common_num)/common_denom;
+        float J_o_4_3 = (-mag_y*mag_N - pd_mN_az*common_num)/common_denom;
+        float J_o_4_4 = (-pd_mN_mx*common_num)/common_denom;
+        float J_o_4_5 = (-imu_az*mag_N - pd_mN_my*common_num)/common_denom;
+        float J_o_4_6 = (imu_ay*mag_N - pd_mN_mz*common_num)/common_denom;
+        J_observation_k << 1.0f, 0, 0, 0, 0, 0,
+                           0, 1.0f, 0, 0, 0, 0,
+                           0, 0, 1.0f, 0, 0, 0,
+                           J_o_4_1, J_o_4_2, J_o_4_3, J_o_4_4, J_o_4_5, J_o_4_6;
+        //printf("- [U] CALC KALMAN GAIN | [J_o_4_1 - J_o_4_6] after calculation, from J_observation_k: [%4.2f | %4.2f | %4.2f | %4.2f | %4.2f | %4.2f]\n", J_observation_k(3, 0), J_observation_k(3, 1), J_observation_k(3, 2), J_observation_k(3, 3), J_observation_k(3, 4), J_observation_k(3, 5));
+        
+        observation_noise_transformed = J_observation_k*accel_mag_covariance*J_observation_k.transpose();
+        //printf("- [U] CALC KALMAN GAIN | Diagonal of observation_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", observation_noise_transformed(0, 0), observation_noise_transformed(1, 1), observation_noise_transformed(2, 2), observation_noise_transformed(3, 3));
+        
+        H_k << -q_k.y(), q_k.z(), -q_k.w(), q_k.x(),
+               q_k.x(), q_k.w(), q_k.z(), q_k.y(),
+               q_k.w(), -q_k.x(), -q_k.y(), q_k.z(),
+               q_k.z(), q_k.y(), q_k.x(), q_k.w();
+        H_k *= 2;
+        //printf("- [U] CALC KALMAN GAIN | First column of H_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", H_k(0, 0), H_k(1, 0), H_k(2, 0), H_k(3, 0));
+        
+        K_k = P_k*H_k.transpose()*(H_k*P_k*H_k.transpose() + observation_noise_transformed).inverse();
+        //printf("- [U] CALC KALMAN GAIN | First column of K_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", K_k(0, 0), K_k(1, 0), K_k(2, 0), K_k(3, 0));
+
+        //UPDATE | Calculate current state estimate based on accel/mag measurements
+        z_k << imu_ax, imu_ay, imu_az, ((imu_ay*mag_z - imu_az*mag_y)/mag_N);
+        z_k_est = H_k*q_k_est.coeffs();                 //TODO: Confirm quaternion multiplication, output and check
+        q_k = q_k_est.coeffs() + K_k*(z_k - z_k_est);   //TODO: Confirm quaternion arithmetic, output and check
+        q_k.normalize();    //Ensure state output is a proper rotation quaternion
+        //printf("- [U] CALC STATE ESTIMATE | See below block for direct output\n");
+
+        //UPDATE | Calculate state covariance based on accel/mag measurements
+        P_k = (I_4 - K_k*H_k)*P_k_est;
+        //printf("- [U] CALC STATE COVAR | First column of P_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k(0, 0), P_k(1, 1), P_k(2, 2), P_k(3, 3));
+
+        //printf("-------- COMPLETED UPDATE COMPUTATION! --------\n\n");
+        //if (orient_count == ORIENTATION_ESTIMATION_RATE_HZ) {
+            printf("~~> Estimate (q_k) Coefficients [w, x, y, z]: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k.w(), q_k.x(), q_k.y(), q_k.z());
+            Vector3f testVect = q_k.toRotationMatrix().eulerAngles(2, 1, 0)*180.0f/M_PI;
+            printf("~~> Estimate (q_k) in Euler [Yaw, Pitch, Roll]: [%3.3f, %3.3f, %3.3f]\n", testVect[0], testVect[1], testVect[2]);
+            Vector3f testAccelVect(imu_ax, imu_ay, imu_az);
+            Vector3f testAccelVectRotated = q_k._transformVector(testAccelVect);
+            printf("~~> Estimate (q_k) rotated accel magnitude vector: [%2.4f, %2.4f, %2.4f]\n\n", testAccelVectRotated[0], testAccelVectRotated[1], testAccelVectRotated[2]);
+
+        //    orient_count = 0;
+        //}
+        //orient_count++;
+
+        //printf("-------- BEGINNING PREDICT COMPUTATION --------\n");
+
+        //Predict next state based on gyroscope measurements
+        gyro_skew << 0, -imu_gx, -imu_gy, -imu_gz,
+                     imu_gx, 0, imu_gz, -imu_gy,
+                     imu_gy, -imu_gz, 0, imu_gx,
+                     imu_gz, imu_gy, -imu_gx, 0;
+        //printf("- [P] PREDICT STATE | Top second element of gyro_skew after set: [%4.2f]\n", gyro_skew(0, 1));
+        
+        F_k = I_4 + (0.5f/ORIENTATION_ESTIMATION_RATE_HZ)*gyro_skew;
+        //printf("- [P] PREDICT STATE | First column of F_k after set: [%2.4f, %2.4f, %2.4f, %2.4f]\n", F_k(0, 0), F_k(1, 0), F_k(2, 0), F_k(3, 0));
+        
+        q_k_est = F_k*q_k.coeffs();
+        q_k_est.normalize();    //TODO: CONFIRM normalization necessary at intermediary steps
+        //printf("- [P] PREDICT STATE | State estimate (q_k_est) coefficients after predict step: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k_est.w(), q_k_est.x(), q_k_est.y(), q_k_est.z());
+
+        //Predict next covariance based on gyroscope measurements + Jacobian-transformed measurement variance
+        J_process_k << q_k.x(), q_k.y(), q_k.z(),
+                       -1.0f*q_k.w(), q_k.z(), -1.0f*q_k.y(),
+                       -1.0f*q_k.z(), -1.0f*q_k.w(), q_k.x(),
+                       q_k.y(), -1.0f*q_k.x(), -1.0f*q_k.w();
+        //printf("- [P] PREDICT COVAR | Top left element of J_process_k after computation: [%4.2f]\n", J_process_k(0, 0));
+        
+        process_noise_transformed = std::pow(0.5f/ORIENTATION_ESTIMATION_RATE_HZ, 2) *
+                                    J_process_k*gyro_covariance*J_process_k.transpose();
+        //printf("- [P] PREDICT COVAR | Diagonal of process_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", process_noise_transformed(0, 0), process_noise_transformed(1, 1), process_noise_transformed(2, 2), process_noise_transformed(3, 3));
+        
+        P_k_est = F_k*P_k*F_k.transpose() + process_noise_transformed;
+        //printf("- [P] PREDICT COVAR | Diagonal of P_k_est after prediction computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k_est(0, 0), P_k_est(1, 1), P_k_est(2, 2), P_k_est(3, 3));
+        //printf("-------- PREDICT COMPUTATION COMPLETED! --------\n\n\n");
+    }
+}
+
+static void sample_cmd_func() {
+    static bool sampling = false;
+
+    if (sampling == false) {
+        vTaskSuspendAll();
+
+        xTaskCreate(MS5607::update_ms5607_task, "update_ms5607", 256, &alt, SENSOR_SAMPLE_PRIORITY, &(alt.update_task_handle));
+        xTaskCreate(ADXL375::update_adxl375_task, "update_adxl375", 256, &adxl375, SENSOR_SAMPLE_PRIORITY, &(adxl375.update_task_handle));
+        xTaskCreate(IIM42653::update_iim42653_task, "update_iim42653", 256, &iim42653, SENSOR_SAMPLE_PRIORITY, &(iim42653.update_task_handle));
+        xTaskCreate(MMC5983MA::update_mmc5983ma_task, "update_mmc5983ma", 256, &mmc5983ma, SENSOR_SAMPLE_PRIORITY, &(mmc5983ma.update_task_handle));
+
+        xTaskCreate(MS5607::ms5607_sample_handler, "ms5607_sample_handler", 256, &alt, EVENT_HANDLER_PRIORITY, &(alt.sample_handler_task_handle));
+
+        vTaskCoreAffinitySet( alt.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( adxl375.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( iim42653.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( mmc5983ma.update_task_handle, 0x01 );
+
+        vTaskCoreAffinitySet( alt.sample_handler_task_handle, 0x01 );
+        sampling = true;
+        xTaskResumeAll();
+    } else {
+        printf("Stopping sample!\n");
+        vTaskSuspendAll();
+
+        vTaskDelete(alt.update_task_handle);
+        vTaskDelete(adxl375.update_task_handle);
+        vTaskDelete(iim42653.update_task_handle);
+        vTaskDelete(mmc5983ma.update_task_handle);
+
+        vTaskDelete(alt.sample_handler_task_handle);
+
+        alt.update_task_handle = NULL;
+        adxl375.update_task_handle = NULL;
+        iim42653.update_task_handle = NULL;
+        mmc5983ma.update_task_handle = NULL;
+
+        alt.sample_handler_task_handle = NULL;
+
+        sampling = false;
+        xTaskResumeAll();
+    }
+}
+
+
+static void debug_cmd_func() {
+    if (logging_handle == NULL) {
+        vTaskSuspendAll();
+        xTaskCreate(logging_task, "logging", 256, NULL, LOGGING_PRIORITY, const_cast<TaskHandle_t *>(&logging_handle));
+        vTaskCoreAffinitySet(logging_handle, 0x02);
+        xTaskResumeAll();
+    } else {
+        vTaskSuspendAll();
+        vTaskDelete(logging_handle);
+        logging_handle = NULL;
+        xTaskResumeAll();
+    }
+}
+
+static void circular_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    if (!use_circular_buffer) {
+        logger.initialize_circular_buffer(PAD_BUFFER_SIZE);
+        use_circular_buffer = true;
+    } else {
+        logger.flush_circular_buffer(true);
+        use_circular_buffer = false;
+    }
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void read_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    if (use_circular_buffer) {
+        logger.read_circular_buffer();
+    } else {
+        logger.read_memory();
+    }
+    vTaskResume(logging_handle);
+}
+
+static void populate_log_entry(log_entry_t * log_entry) {
+    log_entry->time_us = time_us_64();
+
+    adc_select_input(4);
+    log_entry->temperature_chip = adc_read();
+    log_entry->state = PAD;
+    log_entry->deploy_percent = 80;
+    log_entry->pressure = alt.get_pressure();
+    log_entry->altitude = alt.get_altitude();
+    log_entry->temperature_alt = alt.get_temperature();
+
+    log_entry->ax = iim42653.get_ax();
+    log_entry->ay = iim42653.get_ay();
+    log_entry->az = iim42653.get_az();
+    log_entry->gx = iim42653.get_gx();
+    log_entry->gy = iim42653.get_gy();
+    log_entry->gz = iim42653.get_gz();
+
+    log_entry->mag_x = mmc5983ma.get_ax();
+    log_entry->mag_y = mmc5983ma.get_ay();
+    log_entry->mag_z = mmc5983ma.get_az();
+
+    log_entry->high_g_x = adxl375.get_ax();
+    log_entry->high_g_y = adxl375.get_ay();
+    log_entry->high_g_z = adxl375.get_az();
+
+    log_entry->data0 = get_rand_64();
+    log_entry->data1 = get_rand_64();
+    log_entry->data2 = get_rand_32();
+    log_entry->data3 = get_rand_32();
+}
+
+static void write_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    uint64_t start = time_us_64();
+    log_entry_t log_entry;
+    populate_log_entry(&log_entry);
+    printf("\nWriting the following entry!\n");
+    print_log_entry(reinterpret_cast<const uint8_t *>(&log_entry));
+    if (use_circular_buffer) {
+        logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(&log_entry));
+    } else {
+        logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true);
+    }
+    uint64_t end = time_us_64();
+    printf("\nTook %" PRIu64 " us to write that entry!\n", (end - start));
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void erase_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    logger.erase_memory();
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void orient_cmd_func() {
+    /*
+    float pitch_rad = std::atan(imu_ay/std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_az, 2)));
+    float roll_rad = std::atan(imu_ax/std::sqrt(std::pow(imu_ay, 2) + std::pow(imu_az, 2)));
+    float yaw_rad = std::atan2(-mag_y*std::cos(roll_rad) + mag_z*std::sin(roll_rad), 
+            mag_x*std::cos(pitch_rad) + mag_y*std::sin(pitch_rad)*std::sin(roll_rad) + mag_z*std::cos(roll_rad) * std::sin(pitch_rad));
+    */
+
+    static TaskHandle_t pose_estimation_handle = NULL;
+    static bool estimating = false;
+
+    if (!estimating) {
+        printf("================ BEGINNING STATE ESTIMATION ================\n");
+        vTaskSuspendAll();
+        
+        //TODO: Assign pose estimation unique priority?
+        xTaskCreate(pose_estimation_task, "pose_estimation", 1024, NULL, 
+                    SENSOR_SAMPLE_PRIORITY, &pose_estimation_handle);
+        vTaskCoreAffinitySet( pose_estimation_handle, 0x01 );
+
+        estimating = true;
+        xTaskResumeAll();
+    } else {
+        printf("================= ENDING STATE ESTIMATION ================\n");
+        vTaskSuspendAll();
+
+        vTaskDelete(pose_estimation_handle);
+        pose_estimation_handle = NULL;
+
+        estimating = false;
+        xTaskResumeAll();
+    }   
+}
+