diff --git a/config/config.json b/config/config.json
index 9839777..6bdb325 100644
--- a/config/config.json
+++ b/config/config.json
@@ -35,7 +35,7 @@
         "dt_rate_hz": 1000,
         "apply_vectoring_in_regen": true
     },
-    "VNDriver": {
+    "vn_driver": {
         "device_name": "/dev/ttyUSB1",
         "baud_rate": 921600, 
         "port": 1,
diff --git a/drivebrain_comms/CMakeLists.txt b/drivebrain_comms/CMakeLists.txt
index 54ba86b..6e8ab39 100644
--- a/drivebrain_comms/CMakeLists.txt
+++ b/drivebrain_comms/CMakeLists.txt
@@ -9,6 +9,7 @@ find_package(dbcppp REQUIRED)
 
 add_library(drivebrain_comms
     src/CANComms.cpp
+    src/VNComms.cpp
     src/ETHSendComms.cpp
 )
 
@@ -24,4 +25,4 @@ target_link_libraries(drivebrain_comms PUBLIC
     hytech_msgs_cpp_lib
     libvncxx
     dbcppp::dbcppp
-)
+)
\ No newline at end of file
diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp
new file mode 100644
index 0000000..2cdbbc2
--- /dev/null
+++ b/drivebrain_comms/include/VNComms.hpp
@@ -0,0 +1,76 @@
+#pragma once
+
+#include <StateTracker.hpp>
+
+// protobuf
+#include "FoxgloveServer.hpp"
+#include <google/protobuf/any.pb.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/dynamic_message.h>
+#include "hytech_msgs.pb.h"
+
+// boost
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+
+// c++ stl includes
+#include <memory>
+#include <deque>
+#include <variant>
+#include <mutex>
+#include <thread>
+#include <condition_variable>
+#include <functional>
+#include <optional>
+
+#include <unistd.h>
+#include <cstring>
+
+#include "libvncxx/vntime.h"
+#include "libvncxx/packetfinder.h"
+#include "libvncxx/packet.h"
+#include <spdlog/spdlog.h>
+
+using namespace vn::xplat;
+using namespace vn::protocol::uart;
+using SerialPort = boost::asio::serial_port;
+
+namespace comms {
+    class VNDriver
+    {
+        public:
+            VNDriver(std::shared_ptr<core::StateTracker> state_tracker, boost::asio::io_context &io_context, bool &init_successful); 
+            ~VNDriver() {
+                spdlog::info("destructed %s");
+            }
+            bool init();
+            struct config {
+                int baud_rate;
+                int freq_divisor;
+            };
+
+        private: 
+            std::shared_ptr<core::StateTracker> _state_tracker;
+            vn::protocol::uart::PacketFinder _processor;
+            boost::array<std::uint8_t, 512> _output_buff;
+            boost::array<std::uint8_t, 512> _input_buff;
+            SerialPort _serial;
+            config _config;
+            bool _initial_heading_set = false;
+            float _local_declination;
+
+        public: 
+            // Public methods
+            void log_proto_message(std::shared_ptr<google::protobuf::Message> msg);  
+            
+        private:
+            // Private methods
+            static void _handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts);
+            void _configure_INS();
+            void _configure_binary_outputs();
+            void _start_recieve();
+            void _set_initial_heading(float initial_heading);
+            void _try_initialize_heading(float mag_heading, uint8_t ins_mode);
+
+    };
+}
diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp
new file mode 100644
index 0000000..0f7e6d9
--- /dev/null
+++ b/drivebrain_comms/src/VNComms.cpp
@@ -0,0 +1,290 @@
+#include <VNComms.hpp>
+
+// standard includes
+#include <iostream>
+#include <cstring>
+#include <cerrno>
+#include <cctype>
+
+#include "hytech_msgs.pb.h"
+#include "base_msgs.pb.h"
+
+#include "libvncxx/types.h"
+#include "libvncxx/vntime.h"
+#include "libvncxx/packetfinder.h"
+#include "libvncxx/packet.h"
+#include <spdlog/spdlog.h>
+
+#include "FoxgloveServer.hpp"
+#include "Telemetry.hpp"
+
+namespace comms
+{
+
+    bool VNDriver::init() {
+        // Try to establish a connection to the driver
+        spdlog::info("Opening vn driver.");
+        auto& foxglove = core::FoxgloveServer::instance();
+
+        auto device_name = foxglove.get_param<std::string>("vn_driver/device_name");
+        auto baud_rate = foxglove.get_param<int>("vn_driver/baud_rate");
+        auto freq_divisor = foxglove.get_param<int>("vn_driver/freq_divisor");
+        auto port = foxglove.get_param<int>("vn_driver/port");
+
+        if(!(device_name && baud_rate && freq_divisor && port)) {
+            spdlog::error("Couldn't load all params for VN Driver");
+            return false;
+        }
+
+        _config.baud_rate = baud_rate.value();
+        _config.freq_divisor = freq_divisor.value();
+
+        _processor.registerPossiblePacketFoundHandler(this, &VNDriver::_handle_recieve);
+        
+        boost::system::error_code ec;
+        auto ec_ret = _serial.open(device_name.value(), ec);
+
+        if (ec) {
+            spdlog::warn("Error: {}", ec.message());
+            spdlog::info("failed to open vectornav serial port");
+            return false;
+        }
+
+        // Set the baud rate of the device along with other configs
+        _serial.set_option(SerialPort::baud_rate(_config.baud_rate));
+        _serial.set_option(SerialPort::character_size(8));
+        _serial.set_option(SerialPort::parity(SerialPort::parity::none));
+        _serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one));
+        _serial.set_option(SerialPort::flow_control(SerialPort::flow_control::none));
+        
+        // Configures the binary outputs for the device
+        spdlog::info("Configuring binary outputs.");
+        _configure_binary_outputs();
+
+        spdlog::info("INS config");
+        // TODO: uncomment
+        // _configure_INS();
+        
+        
+        return true;
+    }
+
+    VNDriver::VNDriver(std::shared_ptr<core::StateTracker> state_tracker, boost::asio::io_context& io, bool &init_not_successful)
+        : _state_tracker(state_tracker),
+          _serial(io) {
+        init_not_successful = !init();
+
+        // Starts read
+        if(!init_not_successful) {
+            spdlog::info("Starting vn driver recieve.");
+            _start_recieve();
+        }
+        
+    }
+
+    void VNDriver::log_proto_message(std::shared_ptr<google::protobuf::Message> msg) {
+        _state_tracker->handle_receive_protobuf_message(static_cast<std::shared_ptr<google::protobuf::Message>>(msg));
+        core::log(static_cast<std::shared_ptr<google::protobuf::Message>>(msg));
+    }
+
+
+    // TODO: FINISH IMPLEMENTATION
+    // Sets the heading estimate to the angle provided by the user, immediately initializing the INS filter and expediting the startup process
+    //      Must be before INS mode 3
+    //      Have to feed initial heading within 5 degrees of True North
+    //      Calculate declination (w/Reg21 & 83) and offset Magnetic North to get True North
+    //      Write result to Reg 161 while VN-300 in Mode 0 to set initial heading and expediate startup to Mode 3
+    // New method to try and get initial heading from the VN-300
+    void VNDriver::_try_initialize_heading(float mag_heading, uint8_t ins_mode) {
+        if (_initial_heading_set) return; 
+    
+        if (ins_mode == 0) {
+            float true_heading = mag_heading + _local_declination;
+
+            _set_initial_heading(true_heading);
+
+            _initial_heading_set = true;
+            spdlog::info("Applied declination of {} deg for True Heading of {}", _local_declination, true_heading);
+        }
+        return;
+    }
+
+    // Sets the heading estimate to the angle provided by the user, immediately initializing the INS filter and expediting the startup process
+    void VNDriver::_set_initial_heading(float initial_heading) {
+        // Packet namespace does not have set intial heading command. Generate the $VNSIH command.
+        // The manual shows the format: $VNSIH,heading
+        #if VN_HAVE_SECURE_CRT
+        size_t length = sprintf_s((char*)_output_buff.data(), _output_buff.size(), "$VNSIH,%.3f", initial_heading);
+        #else
+        size_t length = sprintf((char*)_output_buff.data(), "$VNSIH,%.3f", initial_heading);
+        #endif
+
+        // Create packet Checksum
+        auto num_of_bytes = Packet::finalizeCommand(ErrorDetectionMode::ERRORDETECTIONMODE_NONE, (char*)_output_buff.data(), length);
+
+        // Send command to VN-300
+        boost::asio::async_write(_serial,
+            boost::asio::buffer(_output_buff.data(), num_of_bytes),
+            [](const boost::system::error_code &ec, std::size_t bytes_transferred) {
+            if (!ec) {
+                spdlog::warn("Successfully sent {} bytes.", bytes_transferred);
+            } else {
+                spdlog::error("Error sending data: {}", ec.message());
+            }
+        });
+    }
+
+
+    void VNDriver::_configure_INS() {
+        
+        // TODO: Separate function for this?? Consider if VN-300 is ARHS-enabled by default??
+        // Creating buffer for writing to INS Basic Configuration (Register 67)
+        auto num_of_bytes = Packet::genWriteInsBasicConfiguration(
+            ErrorDetectionMode::ERRORDETECTIONMODE_NONE,
+            (char *)_output_buff.data(),
+            _output_buff.size(),
+            0, // Scenario: AHRS (Attitude Only - no GNSS/INS)
+            1, // Set AhrsAiding: provides the ability to switch to using the magnetometer to stabilize heading during times when GNSS-based heading is unavailable.
+            0  // Estimate Baseline: if enabled, the sensor will auto-populate the GNSS Compass Estimated Baseline register (Register 97) with the INS-estimated baseline
+        );
+
+        // Write the buffer to the VN-300
+        boost::asio::async_write(_serial,
+            boost::asio::buffer(_output_buff.data(), num_of_bytes),
+            [](const boost::system::error_code &ec, std::size_t bytes_transferred) {
+            if (!ec) {
+                spdlog::warn("Successfully sent {} bytes.", bytes_transferred);
+            } else {
+                spdlog::error("Error sending data: {}", ec.message());
+            }
+        });
+
+        // TODO: SET WNN HERE
+    }
+
+    void VNDriver::_configure_binary_outputs() {
+        
+        auto num_of_bytes = Packet::genWriteBinaryOutput1(
+            ErrorDetectionMode::ERRORDETECTIONMODE_NONE,
+            (char *)_output_buff.data(),
+            _output_buff.size(),
+            AsyncMode::ASYNCMODE_PORT1,
+            _config.freq_divisor,
+            (CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags.
+            TimeGroup::TIMEGROUP_NONE,
+            ImuGroup::IMUGROUP_UNCOMPACCEL,
+            GpsGroup::GPSGROUP_NONE,
+            AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
+            (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU),
+            GpsGroup::GPSGROUP_NONE);
+            
+            boost::asio::async_write(_serial,
+                boost::asio::buffer(_output_buff.data(), num_of_bytes),
+                [](const boost::system::error_code &ec, std::size_t bytes_transferred) {
+                if (!ec) {
+                    spdlog::warn("Successfully sent {} bytes.", bytes_transferred);
+                } else {
+                    spdlog::error("Error sending data: {}", ec.message());
+                }
+            });
+    }
+
+    void VNDriver::_handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) {
+        auto this_instance = (VNDriver *)userData;
+        if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) {
+            vn::math::vec3f vel;
+            // See if this is a binary packet type we are expecting.
+            if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags.
+            TimeGroup::TIMEGROUP_NONE,
+            ImuGroup::IMUGROUP_UNCOMPACCEL,
+            GpsGroup::GPSGROUP_NONE,
+            AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
+            (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU),
+            GpsGroup::GPSGROUP_NONE)) {
+                spdlog::warn("ERROR: packet is not what we want");
+                return;
+            }
+            
+            // Extract data in correct order
+            auto ypr_data = packet.extractVec3f();
+            auto angular_rate_data = packet.extractVec3f();
+            auto uncomp_accel = packet.extractVec3f();
+            auto linear_accel_body = packet.extractVec3f();
+            uint16_t ins_status = packet.extractUint16();
+            auto pos_lla = packet.extractVec3d();
+            auto vel_body = packet.extractVec3f();
+            auto vel_uncertainty = packet.extractFloat();
+            
+            auto ins_mode = static_cast<hytech_msgs::INSMode>(ins_status & 0b11); // Extract INS mode from status
+
+            // TODO: Attempt to set initial heading
+            // _try_initialize_heading(ypr_data.x, ins_mode);
+
+            // Create the protobuf message to send
+            std::shared_ptr<hytech_msgs::VNData> msg_out = std::make_shared<hytech_msgs::VNData>();
+            
+            hytech_msgs::xyz_vector *linear_vel_msg = msg_out->mutable_vn_vel_m_s();
+            linear_vel_msg->set_x(vel_body.x);
+            linear_vel_msg->set_y(vel_body.y);
+            linear_vel_msg->set_z(vel_body.z);
+            
+            hytech_msgs::xyz_vector *linear_accel_msg = msg_out->mutable_vn_linear_accel_m_ss();
+            linear_accel_msg->set_x(linear_accel_body.x);
+            linear_accel_msg->set_y(linear_accel_body.y);
+            linear_accel_msg->set_z(linear_accel_body.z);
+
+            hytech_msgs::xyz_vector *linear_accel_uncomp_msg = msg_out->mutable_vn_linear_accel_uncomp_m_ss();
+            linear_accel_uncomp_msg->set_x(uncomp_accel.x);
+            linear_accel_uncomp_msg->set_y(uncomp_accel.y);
+            linear_accel_uncomp_msg->set_z(uncomp_accel.z);
+
+            hytech_msgs::xyz_vector *angular_rate_data_msg = msg_out->mutable_vn_angular_rate_rad_s();
+            angular_rate_data_msg->set_x(angular_rate_data.x);
+            angular_rate_data_msg->set_y(angular_rate_data.y);
+            angular_rate_data_msg->set_z(angular_rate_data.z);
+
+            hytech_msgs::ypr_vector *ypr_data_msg = msg_out->mutable_vn_ypr_rad();
+            ypr_data_msg->set_yaw(ypr_data.x);
+            ypr_data_msg->set_pitch(ypr_data.y);
+            ypr_data_msg->set_roll(ypr_data.z);
+
+            hytech_msgs::GPS_data *vn_gps_msg = msg_out->mutable_vn_gps();
+            vn_gps_msg->set_lat(pos_lla.x);
+            vn_gps_msg->set_lon(pos_lla.y);
+
+            hytech_msgs::vn_status *vn_ins_msg = msg_out->mutable_status();
+            vn_ins_msg->set_ins_mode(ins_mode); 
+            vn_ins_msg->set_gnss_fix((ins_status >> 2) & 0b1); 
+            vn_ins_msg->set_error_imu((ins_status >> 4) & 1);
+            vn_ins_msg->set_error_mag_pres((ins_status >> 5) & 0b1);
+            vn_ins_msg->set_error_gnss((ins_status >> 6) & 0b1);
+            vn_ins_msg->set_gnss_heading_ins((ins_status >> 8) & 0b1); 
+            vn_ins_msg->set_gnss_compass((ins_status >> 9) & 0b1); 
+            vn_ins_msg->set_ins_mode_int((ins_status & 0b11));
+            vn_ins_msg->set_ins_vel_u(vel_uncertainty);
+
+            this_instance->log_proto_message(static_cast<std::shared_ptr<google::protobuf::Message>>(msg_out));
+        } else {
+            spdlog::warn("Packet not correct");
+        }
+    }
+
+    void VNDriver::_start_recieve() {
+        _serial.async_read_some(
+            boost::asio::buffer(_input_buff),
+            [&](const boost::system::error_code &ec, std::size_t bytesCount) -> void
+            {
+                if (ec) {
+                    if (ec != boost::asio::error::operation_aborted) {
+                        spdlog::error("ERROR: {}", ec.message());
+                    }
+                    return;
+                }
+                // _logger.log_string("logging", core::LogLevel::INFO);
+                _processor.processReceivedData((char *)(_input_buff.data()), bytesCount);
+                // Initiate another asynchronous read
+                _start_recieve();
+            });
+    }
+
+}
diff --git a/tests/scripts/run_ethernet_driver.cpp b/tests/scripts/run_ethernet_driver.cpp
index 99cf55e..aaab549 100644
--- a/tests/scripts/run_ethernet_driver.cpp
+++ b/tests/scripts/run_ethernet_driver.cpp
@@ -1,63 +1,63 @@
-/**
- * Script for testing ethernet driver functionality.
- * Spawns a mock thread sending protobuf messages over "ethernet" using the ETHSend class.
-*/
+// /**
+//  * Script for testing ethernet driver functionality.
+//  * Spawns a mock thread sending protobuf messages over "ethernet" using the ETHSend class.
+// */
 
-#include "ETHSendComms.hpp"
-#include "ETHRecvComms.hpp"
-#include <boost/asio/io_context.hpp>
-#include <chrono>
-#include <csignal>
-#include <google/protobuf/message.h>
-#include <iostream>
-#include <atomic> 
-#include <hytech_msgs.pb.h>
-#include <hytech.pb.h>
-#include <FoxgloveServer.hpp>
-#include <MCAPLogger.hpp>
-#include <mcap/writer.hpp>
-#include <memory>
-#include <thread>
+// #include "ETHSendComms.hpp"
+// #include "ETHRecvComms.hpp"
+// #include <boost/asio/io_context.hpp>
+// #include <chrono>
+// #include <csignal>
+// #include <google/protobuf/message.h>
+// #include <iostream>
+// #include <atomic> 
+// #include <hytech_msgs.pb.h>
+// #include <hytech.pb.h>
+// #include <FoxgloveServer.hpp>
+// #include <MCAPLogger.hpp>
+// #include <mcap/writer.hpp>
+// #include <memory>
+// #include <thread>
 
-std::atomic<bool> running = true;
-void sig_handler(int signal) {
-    if(signal == SIGINT) {
-        std::cout << "halting\n";
-        running = false;
-    }
-}
+// std::atomic<bool> running = true;
+// void sig_handler(int signal) {
+//     if(signal == SIGINT) {
+//         std::cout << "halting\n";
+//         running = false;
+//     }
+// }
 
-int main(int argc, char* argv[]) {
-    // Singleton Creation
-    core::FoxgloveServer::create(argv[1]);
-    core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]);
-    core::MCAPLogger::instance().open_new_mcap("test_1.mcap");
-    core::MCAPLogger::instance().init_logging();
+// int main(int argc, char* argv[]) {
+//     // Singleton Creation
+//     core::FoxgloveServer::create(argv[1]);
+//     core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]);
+//     core::MCAPLogger::instance().open_new_mcap("test_1.mcap");
+//     core::MCAPLogger::instance().init_logging();
 
-    std::signal(SIGINT, sig_handler);
+//     std::signal(SIGINT, sig_handler);
     
-    boost::asio::io_context io_context;
-    comms::ETHSendComms eth_sender{io_context, 2222, "127.0.0.1"};
-    comms::ETHRecvComms<hytech_msgs::ACUAllData> eth_recver{io_context, 2222};
+//     boost::asio::io_context io_context;
+//     comms::ETHSendComms eth_sender{io_context, 2222, "127.0.0.1"};
+//     comms::ETHRecvComms<hytech_msgs::ACUAllData> eth_recver{io_context, 2222};
 
-    std::thread io_thread([&]() {
-        io_context.run();
-    });
+//     std::thread io_thread([&]() {
+//         io_context.run();
+//     });
 
-    std::thread t1([&]() {
-        while(running) {
-            auto msg = std::make_shared<hytech_msgs::ACUAllData>();
-            msg->set_max_cell_temp_id(6767);
-            std::this_thread::sleep_for((std::chrono::seconds(1)));
-            eth_sender.enqueue_msg_send(msg);
-        }
-    });
+//     std::thread t1([&]() {
+//         while(running) {
+//             auto msg = std::make_shared<hytech_msgs::ACUAllData>();
+//             msg->set_max_cell_temp_id(6767);
+//             std::this_thread::sleep_for((std::chrono::seconds(1)));
+//             eth_sender.enqueue_msg_send(msg);
+//         }
+//     });
 
-    if(t1.joinable()) t1.join();
-    io_context.stop();
-    if(io_thread.joinable()) io_thread.join();
+//     if(t1.joinable()) t1.join();
+//     io_context.stop();
+//     if(io_thread.joinable()) io_thread.join();
     
-    core::MCAPLogger::instance().close_active_mcap();
-    core::MCAPLogger::destroy();
-    core::FoxgloveServer::destroy();
-}
+//     core::MCAPLogger::instance().close_active_mcap();
+//     core::MCAPLogger::destroy();
+//     core::FoxgloveServer::destroy();
+// }
diff --git a/tests/scripts/run_mcap_logger.cpp b/tests/scripts/run_mcap_logger.cpp
index 2e97052..d19bdd9 100644
--- a/tests/scripts/run_mcap_logger.cpp
+++ b/tests/scripts/run_mcap_logger.cpp
@@ -1,61 +1,61 @@
-/**
- * Script for testing MCAP logging functionality.
- * Spawns two mock threads sending protobuf messages at different rates and logs to MCAP.
-*/
-
-#include <chrono>
-#include <csignal>
-#include <google/protobuf/message.h>
-#include <iostream>
-#include <atomic> 
-#include <hytech_msgs.pb.h>
-#include <hytech.pb.h>
-#include <FoxgloveServer.hpp>
-#include <MCAPLogger.hpp>
-#include <mcap/writer.hpp>
-#include <memory>
-#include <thread>
-
-std::atomic<bool> running = true;
-
-void sig_handler(int signal) {
-    if(signal == SIGINT) {
-        spdlog::info("halting");
-        running = false;
-    }
-}
-
-void get_param_task(int wait_time, core::MsgType msg) {
-    while(running) {
-        core::MCAPLogger::instance().log_msg(static_cast<core::MsgType>(msg));
-        std::this_thread::sleep_for((std::chrono::milliseconds(wait_time)));
-    }
-}
-
-int main(int argc, char* argv[]) {
-
-    core::FoxgloveServer::create(argv[1]);
-    core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]);
-    core::MCAPLogger::instance().open_new_mcap("test_1.mcap");
-    core::MCAPLogger::instance().init_logging();
-
-    std::signal(SIGINT, sig_handler);
+// /**
+//  * Script for testing MCAP logging functionality.
+//  * Spawns two mock threads sending protobuf messages at different rates and logs to MCAP.
+// */
+
+// #include <chrono>
+// #include <csignal>
+// #include <google/protobuf/message.h>
+// #include <iostream>
+// #include <atomic> 
+// #include <hytech_msgs.pb.h>
+// #include <hytech.pb.h>
+// #include <FoxgloveServer.hpp>
+// #include <MCAPLogger.hpp>
+// #include <mcap/writer.hpp>
+// #include <memory>
+// #include <thread>
+
+// std::atomic<bool> running = true;
+
+// void sig_handler(int signal) {
+//     if(signal == SIGINT) {
+//         spdlog::info("halting");
+//         running = false;
+//     }
+// }
+
+// void get_param_task(int wait_time, core::MsgType msg) {
+//     while(running) {
+//         core::MCAPLogger::instance().log_msg(static_cast<core::MsgType>(msg));
+//         std::this_thread::sleep_for((std::chrono::milliseconds(wait_time)));
+//     }
+// }
+
+// int main(int argc, char* argv[]) {
+
+//     core::FoxgloveServer::create(argv[1]);
+//     core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]);
+//     core::MCAPLogger::instance().open_new_mcap("test_1.mcap");
+//     core::MCAPLogger::instance().init_logging();
+
+//     std::signal(SIGINT, sig_handler);
     
-    auto vel_msg = std::make_shared<hytech::velocities>();
-    vel_msg->set_velocity_x(1000);
-    vel_msg->set_velocity_y(10000);
+//     auto vel_msg = std::make_shared<hytech::velocities>();
+//     vel_msg->set_velocity_x(1000);
+//     vel_msg->set_velocity_y(10000);
 
-    auto acu_data = std::make_shared<hytech_msgs::ACUAllData>();
-    acu_data->set_max_cell_temp_id(676767);
+//     auto acu_data = std::make_shared<hytech_msgs::ACUAllData>();
+//     acu_data->set_max_cell_temp_id(676767);
 
-    std::thread t1(get_param_task, 20, vel_msg);
-    std::thread t2(get_param_task, 40, acu_data);
+//     std::thread t1(get_param_task, 20, vel_msg);
+//     std::thread t2(get_param_task, 40, acu_data);
 
-    if(t1.joinable()) t1.join();
-    if(t2.joinable()) t2.join();
-    core::MCAPLogger::instance().close_active_mcap();
+//     if(t1.joinable()) t1.join();
+//     if(t2.joinable()) t2.join();
+//     core::MCAPLogger::instance().close_active_mcap();
 
-    core::MCAPLogger::destroy();
-    core::FoxgloveServer::destroy();
+//     core::MCAPLogger::destroy();
+//     core::FoxgloveServer::destroy();
 
-}
+// }
diff --git a/tests/test_vn.cpp b/tests/test_vn.cpp
new file mode 100644
index 0000000..debe1bf
--- /dev/null
+++ b/tests/test_vn.cpp
@@ -0,0 +1,152 @@
+#include <csignal>
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+#include "libvncxx/vntime.h"
+#include "libvncxx/packetfinder.h"
+#include "libvncxx/packet.h"
+#include <cstdint>
+#include <optional>
+#include <iterator>
+#include <iostream>
+
+// #include <thread>
+#include <chrono> // std::chrono::seconds
+
+using SerialPort = boost::asio::serial_port;
+
+using namespace vn::xplat;
+using namespace vn::protocol::uart;
+
+std::chrono::time_point<std::chrono::high_resolution_clock> init = {};
+
+void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) {
+    if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) {
+        vn::math::vec3f vel;
+        // See if this is a binary packet type we are expecting.
+        if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags.
+                                 TimeGroup::TIMEGROUP_NONE,
+                                 ImuGroup::IMUGROUP_UNCOMPACCEL,
+                                 GpsGroup::GPSGROUP_NONE,
+                                 AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
+                                 (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY),
+                                 GpsGroup::GPSGROUP_NONE)) {
+            // Not the type of binary packet we are expecting.
+            std::cout << "ERROR: packet is not what we want" << std::endl;
+            return;
+        }
+
+        // Ok, we have our expected binary output packet. Since there are many
+        // ways to configure the binary data output, the burden is on the user
+        // to correctly parse the binary packet. However, we can make use of
+        // the parsing convenience methods provided by the VnUartPacket structure.
+        // When using these convenience methods, you have to extract them in
+        // the order they are organized in the binary packet per the User Manual.
+        auto ypr_data = packet.extractVec3f();
+        auto angular_rate_data = packet.extractVec3f();
+        auto uncomp_accel = packet.extractVec3f();
+        auto linear_accel_body = packet.extractVec3f();
+        uint16_t ins_status = packet.extractUint16();
+        auto pos_lla = packet.extractVec3d();
+        auto vel_body = packet.extractVec3f();
+
+        std::cout << "got data" << std::endl;
+        std::cout << "ypr " << ypr_data.x << " " << ypr_data.y << " " << ypr_data.z << std::endl;
+        std::cout << "uncomp_accel " << uncomp_accel.x << " " << uncomp_accel.y << " " << uncomp_accel.z << std::endl;
+        std::cout << "linear_accel_body " << linear_accel_body.x << " " << linear_accel_body.y << " " << linear_accel_body.z << std::endl;
+        std::cout << "ins_status " << static_cast<uint16_t>(ins_status &= 0x3) << std::endl;
+        std::cout << "pos_lla " << pos_lla.x << " " << pos_lla.y << " " << pos_lla.z << std::endl;
+        std::cout << "vel_body " << vel_body.x << " " << vel_body.y << " " << vel_body.z << std::endl;
+
+        auto curr = std::chrono::high_resolution_clock::now();
+
+        float millis = std::chrono::duration_cast<std::chrono::microseconds>(curr - init).count();
+
+        std::cout << "dt ms: " << millis << std::endl;
+        init = curr;
+    } else {
+        std::cout << "packet not correct" << std::endl;
+    }
+}
+
+void readData(SerialPort &m_serial, boost::array<std::uint8_t, 512> &m_inputBuf, vn::protocol::uart::PacketFinder &processor) {
+    m_serial.async_read_some(
+        boost::asio::buffer(m_inputBuf),
+        [&](const boost::system::error_code &ec, std::size_t bytesCount) {
+            if (ec) {
+                if (ec != boost::asio::error::operation_aborted) {
+                    std::cerr << "ERROR: " << ec.message() << std::endl;
+                }
+                return;
+            }
+
+            processor.processReceivedData((char *)(m_inputBuf.data()), bytesCount);
+            // Initiate another asynchronous read
+            readData(m_serial, m_inputBuf, processor);
+        });
+}
+
+void write_data(SerialPort &m_serial, boost::array<std::uint8_t, 512> &m_outputBuf) {
+    boost::array<std::uint8_t, 512> m_inputBuf;
+
+    // Generate binary output data to write
+    auto numOfBytes = Packet::genWriteBinaryOutput1(
+        ErrorDetectionMode::ERRORDETECTIONMODE_NONE,
+        (char *)m_outputBuf.data(),
+        m_outputBuf.size(),
+        AsyncMode::ASYNCMODE_PORT1,
+        1,
+        (CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Binary OR for flags.
+        TimeGroup::TIMEGROUP_NONE,
+        ImuGroup::IMUGROUP_UNCOMPACCEL,
+        GpsGroup::GPSGROUP_NONE,
+        AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
+        (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY),
+        GpsGroup::GPSGROUP_NONE);
+
+    // Perform another blocking write
+    try {
+        std::size_t bytes_written = boost::asio::write(m_serial, boost::asio::buffer(m_outputBuf.data(), numOfBytes));
+        std::cout << "Successfully sent " << bytes_written << " bytes.\n";
+    } catch (const boost::system::system_error &e) {
+        std::cerr << "Error sending data: " << e.what() << "\n";
+    }
+
+    // Perform another blocking read
+    try {
+        std::size_t bytes_read = m_serial.read_some(boost::asio::buffer(m_inputBuf));
+        // Process the received data
+        // processor.processReceivedData((char *)(m_inputBuf.data()), bytes_read);
+        std::cout <<"read"<<std::endl;
+    } catch (const boost::system::system_error &e) {
+        std::cerr << "Error reading data: " << e.what() << "\n";
+    }
+}
+
+int main() {
+    vn::protocol::uart::PacketFinder processor;
+    processor.registerPossiblePacketFoundHandler(nullptr, validPacketFoundHandler);
+    boost::asio::io_service io;
+    SerialPort m_serial(io);
+    boost::system::error_code ec;
+    std::string dev = "/dev/ttyUSB0";
+    m_serial.open(dev, ec);
+
+    if (ec) {
+        std::cerr << "ERROR: Failed to open " << dev << std::endl;
+        return 1;
+    }
+    m_serial.set_option(SerialPort::baud_rate(921600));
+    m_serial.set_option(SerialPort::character_size(8));
+    m_serial.set_option(SerialPort::parity(SerialPort::parity::none));
+    m_serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one));
+    m_serial.set_option(SerialPort::flow_control(SerialPort::flow_control::none));
+
+    boost::array<std::uint8_t, 512> m_inputBuf;
+    boost::array<std::uint8_t, 512> m_oututBuf;
+
+    write_data(m_serial, m_oututBuf);
+    // processor.resetTracking();
+    readData(m_serial, m_inputBuf, processor);
+    io.run();
+    return 0;
+}
\ No newline at end of file