diff --git a/.vscode/settings.json b/.vscode/settings.json
index c263750..c6c0a0a 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -6,5 +6,8 @@
     "**/.settings": true,
     "**/.factorypath": true
   },
-  "C_Cpp.default.configurationProvider": "vscode-wpilib"
+  "C_Cpp.default.configurationProvider": "vscode-wpilib",
+  "files.associations": {
+    "fstream": "cpp"
+  }
 }
\ No newline at end of file
diff --git a/publish.gradle b/publish.gradle
index 2bdb6ad..12dbd2b 100644
--- a/publish.gradle
+++ b/publish.gradle
@@ -68,7 +68,7 @@ task cppHeadersZip(type: Zip) {
 task cppSourceZip(type: Zip) {
     destinationDirectory = outputsFolder
     archiveBaseName = zipBaseName
-    classifier = "source"
+    classifier = "sources"
 
     from(licenseFile) {
         into '/'
diff --git a/src/main/native/cpp/AHRS.cpp b/src/main/native/cpp/AHRS.cpp
new file mode 100644
index 0000000..eadd159
--- /dev/null
+++ b/src/main/native/cpp/AHRS.cpp
@@ -0,0 +1,1732 @@
+/*
+ * AHRS.cpp
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#include "IIOProvider.h"
+#include "IIOCompleteNotification.h"
+#include "IBoardCapabilities.h"
+#include "InertialDataIntegrator.h"
+#include "OffsetTracker.h"
+#include "ContinuousAngleTracker.h"
+#include "RegisterIOSPI.h"
+#include "RegisterIOI2C.h"
+#include "SerialIO.h"
+#include "AHRS.h"
+#include "AHRSProtocol.h"
+#include "Tracer.h"
+#include "SimIO.h"
+
+#include <hal/HAL.h>
+#include <units/base.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include <sstream>
+#include <fstream>
+#include <string>
+#include <iomanip>
+
+using namespace frc;
+using units::unit_cast;
+
+static const uint8_t NAVX_DEFAULT_UPDATE_RATE_HZ = 60;
+static const int YAW_HISTORY_LENGTH = 10;
+static const int16_t DEFAULT_ACCEL_FSR_G = 2;
+static const int16_t DEFAULT_GYRO_FSR_DPS = 2000;
+static const uint32_t DEFAULT_SPI_BITRATE = 500000;
+static const uint8_t NAVX_MXP_I2C_ADDRESS = 0x32;
+
+class AHRSInternal : public IIOCompleteNotification, public IBoardCapabilities
+{
+    AHRS *ahrs;
+    friend class AHRS;
+    AHRSInternal(AHRS *ahrs)
+    {
+        this->ahrs = ahrs;
+    }
+
+    virtual ~AHRSInternal() {}
+
+    /***********************************************************/
+    /* IIOCompleteNotification Interface Implementation        */
+    /***********************************************************/
+
+    void SetYawPitchRoll(IMUProtocol::YPRUpdate &ypr_update, long sensor_timestamp)
+    {
+        ahrs->yaw = ypr_update.yaw;
+        ahrs->pitch = ypr_update.pitch;
+        ahrs->roll = ypr_update.roll;
+        ahrs->compass_heading = ypr_update.compass_heading;
+        ahrs->last_sensor_timestamp = sensor_timestamp;
+    }
+
+    void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate &ahrs_update, long sensor_timestamp)
+    {
+        /* Update base IMU class variables */
+
+        ahrs->yaw = ahrs_update.yaw;
+        ahrs->pitch = ahrs_update.pitch;
+        ahrs->roll = ahrs_update.roll;
+        ahrs->compass_heading = ahrs_update.compass_heading;
+        ahrs->yaw_offset_tracker->UpdateHistory(ahrs_update.yaw);
+
+        /* Update AHRS class variables */
+
+        // 9-axis data
+        ahrs->fused_heading = ahrs_update.fused_heading;
+
+        // Gravity-corrected linear acceleration (world-frame)
+        ahrs->world_linear_accel_x = ahrs_update.linear_accel_x;
+        ahrs->world_linear_accel_y = ahrs_update.linear_accel_y;
+        ahrs->world_linear_accel_z = ahrs_update.linear_accel_z;
+
+        // Gyro/Accelerometer Die Temperature
+        ahrs->mpu_temp_c = ahrs_update.mpu_temp;
+
+        // Barometric Pressure/Altitude
+        ahrs->altitude = ahrs_update.altitude;
+        ahrs->baro_pressure = ahrs_update.barometric_pressure;
+
+        // Status/Motion Detection
+        ahrs->is_moving =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_MOVING) != 0)
+                 ? true
+                 : false);
+        ahrs->is_rotating =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
+                 ? false
+                 : true);
+        ahrs->altitude_valid =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
+                 ? true
+                 : false);
+        ahrs->is_magnetometer_calibrated =
+            (((ahrs_update.cal_status &
+               NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
+                 ? true
+                 : false);
+        ahrs->magnetic_disturbance =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
+                 ? true
+                 : false);
+
+        ahrs->quaternionW = ahrs_update.quat_w;
+        ahrs->quaternionX = ahrs_update.quat_x;
+        ahrs->quaternionY = ahrs_update.quat_y;
+        ahrs->quaternionZ = ahrs_update.quat_z;
+
+        ahrs->last_sensor_timestamp = sensor_timestamp;
+
+        /* Notify external data arrival subscribers, if any. */
+        for (int i = 0; i < MAX_NUM_CALLBACKS; i++)
+        {
+            ITimestampedDataSubscriber *callback = ahrs->callbacks[i];
+            if (callback != NULL)
+            {
+                long system_timestamp = (long)(unit_cast<double>(Timer::GetFPGATimestamp()) * 1000);
+                callback->timestampedDataReceived(system_timestamp,
+                                                  sensor_timestamp,
+                                                  ahrs_update,
+                                                  ahrs->callback_contexts[i]);
+            }
+        }
+
+        ahrs->velocity[0] = ahrs_update.vel_x;
+        ahrs->velocity[1] = ahrs_update.vel_y;
+        ahrs->velocity[2] = ahrs_update.vel_z;
+        ahrs->displacement[0] = ahrs_update.disp_x;
+        ahrs->displacement[1] = ahrs_update.disp_y;
+        ahrs->displacement[2] = ahrs_update.disp_z;
+
+        UpdateBoardStatus(
+            ahrs_update.op_status,
+            ahrs_update.sensor_status,
+            ahrs_update.cal_status,
+            ahrs_update.selftest_status);
+
+        ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw());
+        ahrs->last_sensor_timestamp = sensor_timestamp;
+    }
+
+    void SetRawData(AHRSProtocol::GyroUpdate &raw_data_update, long sensor_timestamp)
+    {
+        ahrs->raw_gyro_x = raw_data_update.gyro_x;
+        ahrs->raw_gyro_y = raw_data_update.gyro_y;
+        ahrs->raw_gyro_z = raw_data_update.gyro_z;
+        ahrs->raw_accel_x = raw_data_update.accel_x;
+        ahrs->raw_accel_y = raw_data_update.accel_y;
+        ahrs->raw_accel_z = raw_data_update.accel_z;
+        ahrs->cal_mag_x = raw_data_update.mag_x;
+        ahrs->cal_mag_y = raw_data_update.mag_y;
+        ahrs->cal_mag_z = raw_data_update.mag_z;
+        ahrs->mpu_temp_c = raw_data_update.temp_c;
+        ahrs->last_sensor_timestamp = sensor_timestamp;
+    }
+
+    void SetAHRSData(AHRSProtocol::AHRSUpdate &ahrs_update, long sensor_timestamp)
+    {
+        /* Update base IMU class variables */
+
+        ahrs->yaw = ahrs_update.yaw;
+        ahrs->pitch = ahrs_update.pitch;
+        ahrs->roll = ahrs_update.roll;
+        ahrs->compass_heading = ahrs_update.compass_heading;
+        ahrs->yaw_offset_tracker->UpdateHistory(ahrs_update.yaw);
+
+        /* Update AHRS class variables */
+
+        // 9-axis data
+        ahrs->fused_heading = ahrs_update.fused_heading;
+
+        // Gravity-corrected linear acceleration (world-frame)
+        ahrs->world_linear_accel_x = ahrs_update.linear_accel_x;
+        ahrs->world_linear_accel_y = ahrs_update.linear_accel_y;
+        ahrs->world_linear_accel_z = ahrs_update.linear_accel_z;
+
+        // Gyro/Accelerometer Die Temperature
+        ahrs->mpu_temp_c = ahrs_update.mpu_temp;
+
+        // Barometric Pressure/Altitude
+        ahrs->altitude = ahrs_update.altitude;
+        ahrs->baro_pressure = ahrs_update.barometric_pressure;
+
+        // Magnetometer Data
+        ahrs->cal_mag_x = ahrs_update.cal_mag_x;
+        ahrs->cal_mag_y = ahrs_update.cal_mag_y;
+        ahrs->cal_mag_z = ahrs_update.cal_mag_z;
+
+        // Status/Motion Detection
+        ahrs->is_moving =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_MOVING) != 0)
+                 ? true
+                 : false);
+        ahrs->is_rotating =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
+                 ? false
+                 : true);
+        ahrs->altitude_valid =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
+                 ? true
+                 : false);
+        ahrs->is_magnetometer_calibrated =
+            (((ahrs_update.cal_status &
+               NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
+                 ? true
+                 : false);
+        ahrs->magnetic_disturbance =
+            (((ahrs_update.sensor_status &
+               NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
+                 ? true
+                 : false);
+
+        ahrs->quaternionW = ahrs_update.quat_w;
+        ahrs->quaternionX = ahrs_update.quat_x;
+        ahrs->quaternionY = ahrs_update.quat_y;
+        ahrs->quaternionZ = ahrs_update.quat_z;
+
+        ahrs->last_sensor_timestamp = sensor_timestamp;
+
+        /* Notify external data arrival subscribers, if any. */
+        for (int i = 0; i < MAX_NUM_CALLBACKS; i++)
+        {
+            ITimestampedDataSubscriber *callback = ahrs->callbacks[i];
+            if (callback != NULL)
+            {
+                long system_timestamp = (long)(unit_cast<double>(Timer::GetFPGATimestamp()) * 1000);
+                callback->timestampedDataReceived(system_timestamp,
+                                                  sensor_timestamp,
+                                                  ahrs_update,
+                                                  ahrs->callback_contexts[i]);
+            }
+        }
+
+        UpdateBoardStatus(
+            ahrs_update.op_status,
+            ahrs_update.sensor_status,
+            ahrs_update.cal_status,
+            ahrs_update.selftest_status);
+
+        ahrs->UpdateDisplacement(ahrs->world_linear_accel_x,
+                                 ahrs->world_linear_accel_y,
+                                 ahrs->update_rate_hz,
+                                 ahrs->is_moving);
+
+        ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw());
+    }
+
+    void SetBoardID(AHRSProtocol::BoardID &board_id)
+    {
+        ahrs->board_type = board_id.type;
+        ahrs->hw_rev = board_id.hw_rev;
+        ahrs->fw_ver_major = board_id.fw_ver_major;
+        ahrs->fw_ver_minor = board_id.fw_ver_minor;
+        const char *p_boardtype = "unknown";
+        if (board_id.type == 50)
+        {
+            p_boardtype = "navX-Sensor";
+        }
+        if (board_id.hw_rev == 33)
+        {
+            p_boardtype = "navX-MXP (Classic)";
+        }
+        else if (board_id.hw_rev == 34)
+        {
+            p_boardtype = "navX2-MXP (Gen 2)";
+        }
+        else if (board_id.hw_rev == 40)
+        {
+            p_boardtype = "navX-Micro (Classic)";
+        }
+        else if (board_id.hw_rev == 41)
+        {
+            p_boardtype = "navX2-Micro (Gen 2)";
+        }
+        else if ((board_id.hw_rev >= 60) && (board_id.hw_rev <= 69))
+        {
+            p_boardtype = "VMX-pi";
+        }
+        Tracer::Trace("navX-Sensor Board Type %d (%s)\n", board_id.type, p_boardtype);
+        Tracer::Trace("navX-Sensor firmware version %d.%d\n", board_id.fw_ver_major, board_id.fw_ver_minor);
+    }
+
+    void SetBoardState(IIOCompleteNotification::BoardState &board_state, bool update_board_status)
+    {
+        ahrs->update_rate_hz = board_state.update_rate_hz;
+        ahrs->accel_fsr_g = board_state.accel_fsr_g;
+        ahrs->gyro_fsr_dps = board_state.gyro_fsr_dps;
+        ahrs->capability_flags = board_state.capability_flags;
+        if (update_board_status)
+        {
+            UpdateBoardStatus(board_state.op_status, board_state.sensor_status, board_state.cal_status, board_state.selftest_status);
+        }
+    }
+
+    void UpdateBoardStatus(uint8_t op_status, int16_t sensor_status, uint8_t cal_status, uint8_t selftest_status)
+    {
+        /* Detect/Report Board operational status transitions */
+        bool poweron_init_completed = false;
+        if (ahrs->op_status == NAVX_OP_STATUS_NORMAL)
+        {
+            if (op_status != NAVX_OP_STATUS_NORMAL)
+            {
+                /* Board reset detected */
+                Tracer::Trace("navX-Sensor Reset Detected.\n");
+            }
+        }
+        else
+        {
+            if (op_status == NAVX_OP_STATUS_NORMAL)
+            {
+                poweron_init_completed = true;
+                if ((cal_status & NAVX_CAL_STATUS_IMU_CAL_STATE_MASK) != NAVX_CAL_STATUS_IMU_CAL_COMPLETE)
+                {
+                    Tracer::Trace("navX-Sensor startup initialization underway; startup calibration in progress.\n");
+                }
+                else
+                {
+                    Tracer::Trace("navX-Sensor startup initialization and startup calibration complete.\n");
+                }
+            }
+        }
+
+        /* Detect/report reset of yaw angle tracker upon transition to startup calibration complete state. */
+        if (((ahrs->cal_status & NAVX_CAL_STATUS_IMU_CAL_STATE_MASK) != NAVX_CAL_STATUS_IMU_CAL_COMPLETE) &&
+            ((cal_status & NAVX_CAL_STATUS_IMU_CAL_STATE_MASK) == NAVX_CAL_STATUS_IMU_CAL_COMPLETE))
+        {
+            Tracer::Trace("navX-Sensor onboard startup calibration complete.\n");
+            /* Carefully, only reset the software yaw reset offset if calibration completion upon */
+            /* initial poweron or after board reset occurs. */
+            if (poweron_init_completed || ahrs->disconnect_startupcalibration_recovery_pending)
+            {
+                ahrs->disconnect_startupcalibration_recovery_pending = false;
+                ahrs->yaw_angle_tracker->Init();
+                Tracer::Trace("navX-Sensor Yaw angle auto-reset to 0.0 due to startup calibration.\n");
+            }
+        }
+
+        ahrs->op_status = op_status;
+        ahrs->sensor_status = sensor_status;
+        ahrs->cal_status = cal_status;
+        ahrs->selftest_status = selftest_status;
+    }
+
+    void YawResetComplete()
+    {
+        ahrs->yaw_angle_tracker->Reset();
+        if (ahrs->enable_boardlevel_yawreset)
+        {
+            Tracer::Trace("navX-Sensor Board-level Yaw Reset completed.\n");
+        }
+        else
+        {
+            Tracer::Trace("navX-Sensor Software Yaw Reset completed.\n");
+        }
+    }
+
+    void DisconnectDetected()
+    {
+        /* Board disconnect may be caused by intermittent communication loss, or by board reset. */
+        /* Default status to error */
+        ahrs->op_status = NAVX_OP_STATUS_ERROR;
+        ahrs->sensor_status = 0; /* Clear all sensor status flags */
+        /* Flag the need to watch for a startup calibration completion event upon later reconnect. */
+        ahrs->disconnect_startupcalibration_recovery_pending = true;
+        Tracer::Trace("navX-Sensor DISCONNECTED!!!.\n");
+    }
+
+    void ConnectDetected()
+    {
+        Tracer::Trace("navX-Sensor Connected.\n");
+    }
+
+    /***********************************************************/
+    /* IBoardCapabilities Interface Implementation        */
+    /***********************************************************/
+    bool IsOmniMountSupported()
+    {
+        return (((ahrs->capability_flags & NAVX_CAPABILITY_FLAG_OMNIMOUNT) != 0) ? true : false);
+    }
+
+    bool IsBoardYawResetSupported()
+    {
+        return (((ahrs->capability_flags & NAVX_CAPABILITY_FLAG_YAW_RESET) != 0) ? true : false);
+    }
+
+    bool IsDisplacementSupported()
+    {
+        return (((ahrs->capability_flags & NAVX_CAPABILITY_FLAG_VEL_AND_DISP) != 0) ? true : false);
+    }
+
+    bool IsAHRSPosTimestampSupported()
+    {
+        return (((ahrs->capability_flags & NAVX_CAPABILITY_FLAG_AHRSPOS_TS) != 0) ? true : false);
+    }
+};
+
+/**
+ * The AHRS class provides an interface to AHRS capabilities
+ * of the KauaiLabs navX Robotics Navigation Sensor via SPI, I2C and
+ * Serial (TTL UART and USB) communications interfaces on the RoboRIO.
+ *
+ * The AHRS class enables access to basic connectivity and state information,
+ * as well as key 6-axis and 9-axis orientation information (yaw, pitch, roll,
+ * compass heading, fused (9-axis) heading and magnetic disturbance detection.
+ *
+ * Additionally, the ARHS class also provides access to extended information
+ * including linear acceleration, motion detection, rotation detection and sensor
+ * temperature.
+ *
+ * If used with the navX Aero, the AHRS class also provides access to
+ * altitude, barometric pressure and pressure sensor temperature data
+ * @author Scott
+ */
+
+AHRS::AHRS(frc::SPI::Port spi_port_id, uint8_t update_rate_hz) : m_simDevice("navX-Sensor", spi_port_id)
+{
+    SPIInit(spi_port_id, DEFAULT_SPI_BITRATE, update_rate_hz);
+}
+
+/**
+ * The AHRS class provides an interface to AHRS capabilities
+ * of the KauaiLabs navX Robotics Navigation Sensor via SPI, I2C and
+ * Serial (TTL UART and USB) communications interfaces on the RoboRIO.
+ *
+ * The AHRS class enables access to basic connectivity and state information,
+ * as well as key 6-axis and 9-axis orientation information (yaw, pitch, roll,
+ * compass heading, fused (9-axis) heading and magnetic disturbance detection.
+ *
+ * Additionally, the ARHS class also provides access to extended information
+ * including linear acceleration, motion detection, rotation detection and sensor
+ * temperature.
+ *
+ * If used with the navX Aero, the AHRS class also provides access to
+ * altitude, barometric pressure and pressure sensor temperature data
+ *
+ * This constructor allows the specification of a custom SPI bitrate, in bits/second.
+ *
+ * @author Scott
+ */
+
+AHRS::AHRS(frc::SPI::Port spi_port_id, uint32_t spi_bitrate, uint8_t update_rate_hz) : m_simDevice("navX-Sensor", spi_port_id)
+{
+    SPIInit(spi_port_id, spi_bitrate, update_rate_hz);
+}
+
+/**
+ * Constructs the AHRS class using I2C communication, overriding the
+ * default update rate with a custom rate which may be from 4 to 200,
+ * representing the number of updates per second sent by the sensor.
+ *<p>
+ * This constructor should be used if communicating via I2C.
+ *<p>
+ * Note that increasing the update rate may increase the
+ * CPU utilization.
+ *<p>
+ * @param i2c_port_id I2C Port to use
+ * @param update_rate_hz Custom Update Rate (Hz)
+ */
+AHRS::AHRS(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz) : m_simDevice("navX-Sensor", i2c_port_id)
+{
+    I2CInit(i2c_port_id, update_rate_hz);
+}
+
+/**
+ * Constructs the AHRS class using serial communication, overriding the
+ * default update rate with a custom rate which may be from 4 to 200,
+ * representing the number of updates per second sent by the sensor.
+ *<p>
+ * This constructor should be used if communicating via either
+ * TTL UART or USB Serial interface.
+ *<p>
+ * Note that the serial interfaces can communicate either
+ * processed data, or raw data, but not both simultaneously.
+ * If simultaneous processed and raw data are needed, use
+ * one of the register-based interfaces (SPI or I2C).
+ *<p>
+ * Note that increasing the update rate may increase the
+ * CPU utilization.
+ *<p>
+ * @param serial_port_id SerialPort to use
+ * @param data_type either kProcessedData or kRawData
+ * @param update_rate_hz Custom Update Rate (Hz)
+ */
+AHRS::AHRS(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz) : m_simDevice("navX-Sensor", serial_port_id)
+{
+    SerialInit(serial_port_id, data_type, update_rate_hz);
+}
+
+/**
+ * Constructs the AHRS class using SPI communication and the default update rate.
+ *<p>
+ * This constructor should be used if communicating via SPI.
+ *<p>
+ * @param spi_port_id SPI port to use.
+ */
+AHRS::AHRS(frc::SPI::Port spi_port_id) : m_simDevice("navX-Sensor", spi_port_id)
+{
+    SPIInit(spi_port_id, DEFAULT_SPI_BITRATE, NAVX_DEFAULT_UPDATE_RATE_HZ);
+}
+
+/**
+ * Constructs the AHRS class using I2C communication and the default update rate.
+ *<p>
+ * This constructor should be used if communicating via I2C.
+ *<p>
+ * @param i2c_port_id I2C port to use
+ */
+AHRS::AHRS(frc::I2C::Port i2c_port_id) : m_simDevice("navX-Sensor", i2c_port_id)
+{
+    I2CInit(i2c_port_id, NAVX_DEFAULT_UPDATE_RATE_HZ);
+}
+
+/**
+ * Constructs the AHRS class using serial communication and the default update rate,
+ * and returning processed (rather than raw) data.
+ *<p>
+ * This constructor should be used if communicating via either
+ * TTL UART or USB Serial interface.
+ *<p>
+ * @param serial_port_id SerialPort to use
+ */
+AHRS::AHRS(frc::SerialPort::Port serial_port_id) : m_simDevice("navX-Sensor", serial_port_id)
+{
+    SerialInit(serial_port_id, SerialDataType::kProcessedData, NAVX_DEFAULT_UPDATE_RATE_HZ);
+}
+
+/**
+ * Returns the current pitch value (in degrees, from -180 to 180)
+ * reported by the sensor.  Pitch is a measure of rotation around
+ * the X Axis.
+ * @return The current pitch value in degrees (-180 to 180).
+ */
+float AHRS::GetPitch()
+{
+    return pitch;
+}
+
+/**
+ * Returns the current roll value (in degrees, from -180 to 180)
+ * reported by the sensor.  Roll is a measure of rotation around
+ * the X Axis.
+ * @return The current roll value in degrees (-180 to 180).
+ */
+float AHRS::GetRoll()
+{
+    return roll;
+}
+
+/**
+ * Returns the current yaw value (in degrees, from -180 to 180)
+ * reported by the sensor.  Yaw is a measure of rotation around
+ * the Z Axis (which is perpendicular to the earth).
+ *<p>
+ * Note that the returned yaw value will be offset by a user-specified
+ * offset value; this user-specified offset value is set by
+ * invoking the zeroYaw() method.
+ * @return The current yaw value in degrees (-180 to 180).
+ */
+float AHRS::GetYaw()
+{
+    if (enable_boardlevel_yawreset && ahrs_internal->IsBoardYawResetSupported())
+    {
+        return this->yaw;
+    }
+    else
+    {
+        return (float)yaw_offset_tracker->ApplyOffset(this->yaw);
+    }
+}
+
+/**
+ * Returns the current tilt-compensated compass heading
+ * value (in degrees, from 0 to 360) reported by the sensor.
+ *<p>
+ * Note that this value is sensed by a magnetometer,
+ * which can be affected by nearby magnetic fields (e.g., the
+ * magnetic fields generated by nearby motors).
+ *<p>
+ * Before using this value, ensure that (a) the magnetometer
+ * has been calibrated and (b) that a magnetic disturbance is
+ * not taking place at the instant when the compass heading
+ * was generated.
+ * @return The current tilt-compensated compass heading, in degrees (0-360).
+ */
+float AHRS::GetCompassHeading()
+{
+    return compass_heading;
+}
+
+#define NUM_SUPPRESSED_SUCCESSIVE_YAWRESET_MESSAGES 5
+#define SUPPRESSED_SUCESSIVE_YAWRESET_PERIOD_SECONDS 0.2
+
+/**
+ * Sets the user-specified yaw offset to the current
+ * yaw value reported by the sensor.
+ *<p>
+ * This user-specified yaw offset is automatically
+ * subtracted from subsequent yaw values reported by
+ * the getYaw() method.
+ *
+ * NOTE:  This method has no effect if the sensor is
+ * currently calibrating, since resetting the yaw will
+ * interfere with the calibration process.
+ */
+void AHRS::ZeroYaw()
+{
+    double curr_timestamp = unit_cast<double>(Timer::GetFPGATimestamp());
+    double delta_time_since_last_yawreset_request = curr_timestamp - last_yawreset_request_timestamp;
+    if (delta_time_since_last_yawreset_request < SUPPRESSED_SUCESSIVE_YAWRESET_PERIOD_SECONDS)
+    {
+        successive_suppressed_yawreset_request_count++;
+        if ((successive_suppressed_yawreset_request_count % NUM_SUPPRESSED_SUCCESSIVE_YAWRESET_MESSAGES) == 1)
+        {
+            if (logging_enabled)
+                Tracer::Trace("navX-Sensor rapidly-repeated Yaw Reset ignored%s\n",
+                              ((successive_suppressed_yawreset_request_count < NUM_SUPPRESSED_SUCCESSIVE_YAWRESET_MESSAGES)
+                                   ? "."
+                                   : (" (repeated messages suppressed).")));
+        }
+        return;
+    }
+
+    if (IsCalibrating())
+    {
+        double delta_time_since_last_yawreset_while_calibrating_request =
+            curr_timestamp - last_yawreset_while_calibrating_request_timestamp;
+        if ((delta_time_since_last_yawreset_while_calibrating_request > SUPPRESSED_SUCESSIVE_YAWRESET_PERIOD_SECONDS))
+        {
+            Tracer::Trace("navX-Sensor Yaw Reset request ignored - startup calibration is currently in progress.\n");
+        }
+        last_yawreset_while_calibrating_request_timestamp = curr_timestamp;
+        return;
+    }
+
+    successive_suppressed_yawreset_request_count = 0;
+
+    last_yawreset_request_timestamp = curr_timestamp;
+    if (enable_boardlevel_yawreset && ahrs_internal->IsBoardYawResetSupported())
+    {
+        io->ZeroYaw();
+        Tracer::Trace("navX-Sensor Board-level Yaw Reset requested.\n");
+        /* Note:  Notification is deferred until action is complete. */
+    }
+    else
+    {
+        yaw_offset_tracker->SetOffset();
+        /* Notification occurs immediately. */
+        ahrs_internal->YawResetComplete();
+    }
+}
+
+/**
+ * Returns true if the sensor is currently performing automatic
+ * gyro/accelerometer calibration.  Automatic calibration occurs
+ * when the sensor is initially powered on, during which time the
+ * sensor should be held still, with the Z-axis pointing up
+ * (perpendicular to the earth).
+ *<p>
+ * NOTE:  During this automatic calibration, the yaw, pitch and roll
+ * values returned may not be accurate.
+ *<p>
+ * Once calibration is complete, the sensor will automatically remove
+ * an internal yaw offset value from all reported values.
+ *<p>
+ * @return Returns true if the sensor is currently automatically
+ * calibrating the gyro and accelerometer sensors.
+ */
+bool AHRS::IsCalibrating()
+{
+    return !((cal_status &
+              NAVX_CAL_STATUS_IMU_CAL_STATE_MASK) ==
+             NAVX_CAL_STATUS_IMU_CAL_COMPLETE);
+}
+
+/**
+ * Indicates whether the sensor is currently connected
+ * to the host computer.  A connection is considered established
+ * whenever communication with the sensor has occurred recently.
+ *<p>
+ * @return Returns true if a valid update has been recently received
+ * from the sensor.
+ */
+bool AHRS::IsConnected()
+{
+    return io->IsConnected();
+}
+
+/**
+ * Returns the count in bytes of data received from the
+ * sensor.  This could can be useful for diagnosing
+ * connectivity issues.
+ *<p>
+ * If the byte count is increasing, but the update count
+ * (see getUpdateCount()) is not, this indicates a software
+ * misconfiguration.
+ * @return The number of bytes received from the sensor.
+ */
+double AHRS::GetByteCount()
+{
+    return io->GetByteCount();
+}
+
+/**
+ * Returns the count of valid updates which have
+ * been received from the sensor.  This count should increase
+ * at the same rate indicated by the configured update rate.
+ * @return The number of valid updates received from the sensor.
+ */
+double AHRS::GetUpdateCount()
+{
+    return io->GetUpdateCount();
+}
+
+/**
+ * Returns the sensor timestamp corresponding to the
+ * last sample retrieved from the sensor.  Note that this
+ * sensor timestamp is only provided when the Register-based
+ * IO methods (SPI, I2C) are used; sensor timestamps are not
+ * provided when Serial-based IO methods (TTL UART, USB)
+ * are used.
+ * @return The sensor timestamp corresponding to the current AHRS sensor data.
+ */
+long AHRS::GetLastSensorTimestamp()
+{
+    return this->last_sensor_timestamp;
+}
+
+/**
+ * Returns the current linear acceleration in the X-axis (in G).
+ *<p>
+ * World linear acceleration refers to raw acceleration data, which
+ * has had the gravity component removed, and which has been rotated to
+ * the same reference frame as the current yaw value.  The resulting
+ * value represents the current acceleration in the x-axis of the
+ * body (e.g., the robot) on which the sensor is mounted.
+ *<p>
+ * @return Current world linear acceleration in the X-axis (in G).
+ */
+float AHRS::GetWorldLinearAccelX()
+{
+    return this->world_linear_accel_x;
+}
+
+/**
+ * Returns the current linear acceleration in the Y-axis (in G).
+ *<p>
+ * World linear acceleration refers to raw acceleration data, which
+ * has had the gravity component removed, and which has been rotated to
+ * the same reference frame as the current yaw value.  The resulting
+ * value represents the current acceleration in the Y-axis of the
+ * body (e.g., the robot) on which the sensor is mounted.
+ *<p>
+ * @return Current world linear acceleration in the Y-axis (in G).
+ */
+float AHRS::GetWorldLinearAccelY()
+{
+    return this->world_linear_accel_y;
+}
+
+/**
+ * Returns the current linear acceleration in the Z-axis (in G).
+ *<p>
+ * World linear acceleration refers to raw acceleration data, which
+ * has had the gravity component removed, and which has been rotated to
+ * the same reference frame as the current yaw value.  The resulting
+ * value represents the current acceleration in the Z-axis of the
+ * body (e.g., the robot) on which the sensor is mounted.
+ *<p>
+ * @return Current world linear acceleration in the Z-axis (in G).
+ */
+float AHRS::GetWorldLinearAccelZ()
+{
+    return this->world_linear_accel_z;
+}
+
+/**
+ * Indicates if the sensor is currently detecting motion,
+ * based upon the X and Y-axis world linear acceleration values.
+ * If the sum of the absolute values of the X and Y axis exceed
+ * a "motion threshold", the motion state is indicated.
+ *<p>
+ * @return Returns true if the sensor is currently detecting motion.
+ */
+bool AHRS::IsMoving()
+{
+    return is_moving;
+}
+
+/**
+ * Indicates if the sensor is currently detecting yaw rotation,
+ * based upon whether the change in yaw over the last second
+ * exceeds the "Rotation Threshold."
+ *<p>
+ * Yaw Rotation can occur either when the sensor is rotating, or
+ * when the sensor is not rotating AND the current gyro calibration
+ * is insufficiently calibrated to yield the standard yaw drift rate.
+ *<p>
+ * @return Returns true if the sensor is currently detecting motion.
+ */
+bool AHRS::IsRotating()
+{
+    return is_rotating;
+}
+
+/**
+ * Returns the current barometric pressure, based upon calibrated readings
+ * from the onboard pressure sensor.  This value is in units of millibar.
+ *<p>
+ * NOTE:  This value is only valid for a navX Aero.  To determine
+ * whether this value is valid, see isAltitudeValid().
+ * @return Returns current barometric pressure (navX Aero only).
+ */
+float AHRS::GetBarometricPressure()
+{
+    return baro_pressure;
+}
+
+/**
+ * Returns the current altitude, based upon calibrated readings
+ * from a barometric pressure sensor, and the currently-configured
+ * sea-level barometric pressure [navX Aero only].  This value is in units of meters.
+ *<p>
+ * NOTE:  This value is only valid sensors including a pressure
+ * sensor.  To determine whether this value is valid, see
+ * isAltitudeValid().
+ *<p>
+ * @return Returns current altitude in meters (as long as the sensor includes
+ * an installed on-board pressure sensor).
+ */
+float AHRS::GetAltitude()
+{
+    return altitude;
+}
+
+/**
+ * Indicates whether the current altitude (and barometric pressure) data is
+ * valid. This value will only be true for a sensor with an onboard
+ * pressure sensor installed.
+ *<p>
+ * If this value is false for a board with an installed pressure sensor,
+ * this indicates a malfunction of the onboard pressure sensor.
+ *<p>
+ * @return Returns true if a working pressure sensor is installed.
+ */
+bool AHRS::IsAltitudeValid()
+{
+    return this->altitude_valid;
+}
+
+/**
+ * Returns the "fused" (9-axis) heading.
+ *<p>
+ * The 9-axis heading is the fusion of the yaw angle, the tilt-corrected
+ * compass heading, and magnetic disturbance detection.  Note that the
+ * magnetometer calibration procedure is required in order to
+ * achieve valid 9-axis headings.
+ *<p>
+ * The 9-axis Heading represents the sensor's best estimate of current heading,
+ * based upon the last known valid Compass Angle, and updated by the change in the
+ * Yaw Angle since the last known valid Compass Angle.  The last known valid Compass
+ * Angle is updated whenever a Calibrated Compass Angle is read and the sensor
+ * has recently rotated less than the Compass Noise Bandwidth (~2 degrees).
+ * @return Fused Heading in Degrees (range 0-360)
+ */
+float AHRS::GetFusedHeading()
+{
+    return fused_heading;
+}
+
+/**
+ * Indicates whether the current magnetic field strength diverges from the
+ * calibrated value for the earth's magnetic field by more than the currently-
+ * configured Magnetic Disturbance Ratio.
+ *<p>
+ * This function will always return false if the sensor's magnetometer has
+ * not yet been calibrated; see isMagnetometerCalibrated().
+ * @return true if a magnetic disturbance is detected (or the magnetometer is uncalibrated).
+ */
+bool AHRS::IsMagneticDisturbance()
+{
+    return magnetic_disturbance;
+}
+
+/**
+ * Indicates whether the magnetometer has been calibrated.
+ *<p>
+ * Magnetometer Calibration must be performed by the user.
+ *<p>
+ * Note that if this function does indicate the magnetometer is calibrated,
+ * this does not necessarily mean that the calibration quality is sufficient
+ * to yield valid compass headings.
+ *<p>
+ * @return Returns true if magnetometer calibration has been performed.
+ */
+bool AHRS::IsMagnetometerCalibrated()
+{
+    return is_magnetometer_calibrated;
+}
+
+/* Unit Quaternions */
+
+/**
+ * Returns the imaginary portion (W) of the Orientation Quaternion which
+ * fully describes the current sensor orientation with respect to the
+ * reference angle defined as the angle at which the yaw was last "zeroed".
+ *<p>
+ * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
+ * to 2.  This total range (4) can be associated with a unit circle, since
+ * each circle is comprised of 4 PI Radians.
+ * <p>
+ * For more information on Quaternions and their use, please see this <a href=https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>definition</a>.
+ * @return Returns the imaginary portion (W) of the quaternion.
+ */
+float AHRS::GetQuaternionW()
+{
+    return quaternionW;
+}
+/**
+ * Returns the real portion (X axis) of the Orientation Quaternion which
+ * fully describes the current sensor orientation with respect to the
+ * reference angle defined as the angle at which the yaw was last "zeroed".
+ * <p>
+ * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
+ * to 2.  This total range (4) can be associated with a unit circle, since
+ * each circle is comprised of 4 PI Radians.
+ * <p>
+ * For more information on Quaternions and their use, please see this <a href=https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>description</a>.
+ * @return Returns the real portion (X) of the quaternion.
+ */
+float AHRS::GetQuaternionX()
+{
+    return quaternionX;
+}
+/**
+ * Returns the real portion (X axis) of the Orientation Quaternion which
+ * fully describes the current sensor orientation with respect to the
+ * reference angle defined as the angle at which the yaw was last "zeroed".
+ *
+ * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
+ * to 2.  This total range (4) can be associated with a unit circle, since
+ * each circle is comprised of 4 PI Radians.
+ *
+ * For more information on Quaternions and their use, please see:
+ *
+ *   https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
+ *
+ * @return Returns the real portion (X) of the quaternion.
+ */
+float AHRS::GetQuaternionY()
+{
+    return quaternionY;
+}
+/**
+ * Returns the real portion (X axis) of the Orientation Quaternion which
+ * fully describes the current sensor orientation with respect to the
+ * reference angle defined as the angle at which the yaw was last "zeroed".
+ *
+ * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2
+ * to 2.  This total range (4) can be associated with a unit circle, since
+ * each circle is comprised of 4 PI Radians.
+ *
+ * For more information on Quaternions and their use, please see:
+ *
+ *   https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
+ *
+ * @return Returns the real portion (X) of the quaternion.
+ */
+float AHRS::GetQuaternionZ()
+{
+    return quaternionZ;
+}
+
+/**
+ * Zeros the displacement integration variables.   Invoke this at the moment when
+ * integration begins.
+ */
+void AHRS::ResetDisplacement()
+{
+    if (ahrs_internal->IsDisplacementSupported())
+    {
+        io->ZeroDisplacement();
+    }
+    else
+    {
+        integrator->ResetDisplacement();
+    }
+}
+
+/**
+ * Each time new linear acceleration samples are received, this function should be invoked.
+ * This function transforms acceleration in G to meters/sec^2, then converts this value to
+ * Velocity in meters/sec (based upon velocity in the previous sample).  Finally, this value
+ * is converted to displacement in meters, and integrated.
+ * @return none.
+ */
+
+void AHRS::UpdateDisplacement(float accel_x_g, float accel_y_g,
+                              int update_rate_hz, bool is_moving)
+{
+    integrator->UpdateDisplacement(accel_x_g, accel_y_g, update_rate_hz, is_moving);
+}
+
+/**
+ * Returns the velocity (in meters/sec) of the X axis [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Velocity measures rely on integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting velocities are not known to be very accurate.
+ * @return Current Velocity (in meters/squared).
+ */
+float AHRS::GetVelocityX()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? velocity[0] : integrator->GetVelocityX());
+}
+
+/**
+ * Returns the velocity (in meters/sec) of the Y axis [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Velocity measures rely on integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting velocities are not known to be very accurate.
+ * @return Current Velocity (in meters/squared).
+ */
+float AHRS::GetVelocityY()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? velocity[1] : integrator->GetVelocityY());
+}
+
+/**
+ * Returns the velocity (in meters/sec) of the Z axis [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Velocity measures rely on integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting velocities are not known to be very accurate.
+ * @return Current Velocity (in meters/squared).
+ */
+float AHRS::GetVelocityZ()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? velocity[2] : 0.f);
+}
+
+/**
+ * Returns the displacement (in meters) of the X axis since resetDisplacement()
+ * was last invoked [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Displacement measures rely on double-integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting displacement are not known to be very accurate, and the amount of error
+ * increases quickly as time progresses.
+ * @return Displacement since last reset (in meters).
+ */
+float AHRS::GetDisplacementX()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? displacement[0] : integrator->GetVelocityX());
+}
+
+/**
+ * Returns the displacement (in meters) of the Y axis since resetDisplacement()
+ * was last invoked [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Displacement measures rely on double-integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting displacement are not known to be very accurate, and the amount of error
+ * increases quickly as time progresses.
+ * @return Displacement since last reset (in meters).
+ */
+float AHRS::GetDisplacementY()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? displacement[1] : integrator->GetVelocityY());
+}
+
+/**
+ * Returns the displacement (in meters) of the Z axis since resetDisplacement()
+ * was last invoked [Experimental].
+ *
+ * NOTE:  This feature is experimental.  Displacement measures rely on double-integration
+ * of acceleration values from MEMS accelerometers which yield "noisy" values.  The
+ * resulting displacement are not known to be very accurate, and the amount of error
+ * increases quickly as time progresses.
+ * @return Displacement since last reset (in meters).
+ */
+float AHRS::GetDisplacementZ()
+{
+    return (ahrs_internal->IsDisplacementSupported() ? displacement[2] : 0.f);
+}
+
+/**
+ * Enables or disables logging (via Console I/O) of AHRS library internal
+ * behaviors, including events such as transient communication errors.
+ * @param enable
+ */
+void AHRS::EnableLogging(bool enable)
+{
+    if (this->io != NULL)
+    {
+        io->EnableLogging(enable);
+    }
+    logging_enabled = enable;
+}
+
+/**
+ * Enables or disables board-level yaw zero (reset) requests.  Board-level
+ * yaw resets are processed by the sensor board and the resulting yaw
+ * angle may not be available to the client software until at least
+ * 2 update cycles have occurred.  Board-level yaw resets however do
+ * maintain synchronization between the yaw angle and the sensor-generated
+ * Quaternion and Fused Heading values.
+ *
+ * Conversely, Software-based yaw resets occur instantaneously; however, Software-
+ * based yaw resets do not update the yaw angle component of the sensor-generated
+ * Quaternion values or the Fused Heading values.
+ * @param enable
+ */
+void AHRS::EnableBoardlevelYawReset(bool enable)
+{
+    enable_boardlevel_yawreset = enable;
+}
+
+/**
+ * Returns true if Board-level yaw resets are enabled.  Conversely, returns false
+ * if Software-based yaw resets are active.
+ *
+ * @return true if Board-level yaw resets are enabled.
+ */
+bool AHRS::IsBoardlevelYawResetEnabled()
+{
+    return enable_boardlevel_yawreset;
+}
+
+/**
+ * Returns the sensor full scale range (in degrees per second)
+ * of the X, Y and X-axis gyroscopes.
+ *
+ * @return gyroscope full scale range in degrees/second.
+ */
+int16_t AHRS::GetGyroFullScaleRangeDPS()
+{
+    return gyro_fsr_dps;
+}
+
+/**
+ * Returns the sensor full scale range (in G)
+ * of the X, Y and X-axis accelerometers.
+ *
+ * @return accelerometer full scale range in G.
+ */
+int16_t AHRS::GetAccelFullScaleRangeG()
+{
+    return accel_fsr_g;
+}
+
+#define NAVX_IO_THREAD_NAME "navXIOThread"
+
+void AHRS::SPIInit(SPI::Port spi_port_id, uint32_t bitrate, uint8_t update_rate_hz)
+{
+    Tracer::Trace("Instantiating navX-Sensor on SPI Port %d.\n", spi_port_id);
+    commonInit(update_rate_hz);
+
+    if (m_simDevice)
+    {
+        io = new SimIO(update_rate_hz, ahrs_internal, &m_simDevice);
+    }
+    else
+    {
+        io = new RegisterIO(new RegisterIO_SPI(new SPI(spi_port_id), bitrate), update_rate_hz, ahrs_internal, ahrs_internal);
+    }
+
+    wpi::SendableRegistry::AddLW(this, "navX-Sensor", spi_port_id);
+    task = new std::thread(AHRS::ThreadFunc, io);
+}
+
+void AHRS::I2CInit(I2C::Port i2c_port_id, uint8_t update_rate_hz)
+{
+    Tracer::Trace("Instantiating navX-Sensor on I2C Port %d.\n", i2c_port_id);
+    commonInit(update_rate_hz);
+
+    if (m_simDevice)
+    {
+        io = new SimIO(update_rate_hz, ahrs_internal, &m_simDevice);
+    }
+    else
+    {
+        io = new RegisterIO(new RegisterIO_I2C(new I2C(i2c_port_id, NAVX_MXP_I2C_ADDRESS)), update_rate_hz, ahrs_internal, ahrs_internal);
+    }
+
+    wpi::SendableRegistry::AddLW(this, "navX-Sensor", i2c_port_id);
+    task = new std::thread(AHRS::ThreadFunc, io);
+}
+
+void AHRS::SerialInit(SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz)
+{
+    Tracer::Trace("Instantiating navX-Sensor on Serial Port %d.\n", serial_port_id);
+    commonInit(update_rate_hz);
+
+    if (m_simDevice)
+    {
+        io = new SimIO(update_rate_hz, ahrs_internal, &m_simDevice);
+    }
+    else
+    {
+        bool processed_data = (data_type == SerialDataType::kProcessedData);
+        io = new SerialIO(serial_port_id, update_rate_hz, processed_data, ahrs_internal, ahrs_internal);
+    }
+
+    wpi::SendableRegistry::AddLW(this, "navX-Sensor", serial_port_id);
+    task = new std::thread(AHRS::ThreadFunc, io);
+}
+
+void AHRS::commonInit(uint8_t update_rate_hz)
+{
+
+    Tracer::Trace("navX-Sensor C++ library for FRC\n");
+    HAL_Report(HALUsageReporting::kResourceType_NavX, 0);
+    ahrs_internal = new AHRSInternal(this);
+    this->update_rate_hz = update_rate_hz;
+
+    /* Processed Data */
+
+    yaw_offset_tracker = new OffsetTracker(YAW_HISTORY_LENGTH);
+    integrator = new InertialDataIntegrator();
+    yaw_angle_tracker = new ContinuousAngleTracker();
+
+    yaw = 0.0f;
+    pitch = 0.0f;
+    roll = 0.0f;
+    compass_heading = 0.0f;
+    world_linear_accel_x = 0.0f;
+    world_linear_accel_y = 0.0f;
+    world_linear_accel_z = 0.0f;
+    mpu_temp_c = 0.0f;
+    fused_heading = 0.0f;
+    altitude = 0.0f;
+    baro_pressure = 0.0f;
+    is_moving = false;
+    is_rotating = false;
+    baro_sensor_temp_c = 0.0f;
+    altitude_valid = false;
+    is_magnetometer_calibrated = false;
+    magnetic_disturbance = false;
+    quaternionW = 0.0f;
+    quaternionX = 0.0f;
+    quaternionY = 0.0f;
+    quaternionZ = 0.0f;
+
+    /* Integrated Data */
+
+    for (int i = 0; i < 3; i++)
+    {
+        velocity[i] = 0.0f;
+        displacement[i] = 0.0f;
+    }
+
+    /* Raw Data */
+    raw_gyro_x = 0.0f;
+    raw_gyro_y = 0.0f;
+    raw_gyro_z = 0.0f;
+    raw_accel_x = 0.0f;
+    raw_accel_y = 0.0f;
+    raw_accel_z = 0.0f;
+    cal_mag_x = 0.0f;
+    cal_mag_y = 0.0f;
+    cal_mag_z = 0.0f;
+
+    /* Configuration/Status */
+    update_rate_hz = 0;
+    accel_fsr_g = DEFAULT_ACCEL_FSR_G;
+    gyro_fsr_dps = DEFAULT_GYRO_FSR_DPS;
+    capability_flags = 0;
+    op_status = 0;
+    sensor_status = 0;
+    cal_status = 0;
+    selftest_status = 0;
+    /* Board ID */
+    board_type = 0;
+    hw_rev = 0;
+    fw_ver_major = 0;
+    fw_ver_minor = 0;
+    last_sensor_timestamp = 0;
+    last_update_time = 0;
+
+    io = 0;
+
+    for (int i = 0; i < MAX_NUM_CALLBACKS; i++)
+    {
+        callbacks[i] = NULL;
+        callback_contexts[i] = NULL;
+    }
+
+    enable_boardlevel_yawreset = false;
+    last_yawreset_request_timestamp = 0;
+    last_yawreset_while_calibrating_request_timestamp = 0;
+    successive_suppressed_yawreset_request_count = 0;
+    disconnect_startupcalibration_recovery_pending = false;
+    logging_enabled = false;
+}
+
+/**
+ * Returns the total accumulated yaw angle (Z Axis, in degrees)
+ * reported by the sensor.
+ *<p>
+ * NOTE: The angle is continuous, meaning it's range is beyond 360 degrees.
+ * This ensures that algorithms that wouldn't want to see a discontinuity
+ * in the gyro output as it sweeps past 0 on the second time around.
+ *<p>
+ * Note that the returned yaw value will be offset by a user-specified
+ * offset value; this user-specified offset value is set by
+ * invoking the zeroYaw() method.
+ *<p>
+ * @return The current total accumulated yaw angle (Z axis) of the robot
+ * in degrees. This heading is based on integration of the returned rate
+ * from the Z-axis (yaw) gyro.
+ */
+
+double AHRS::GetAngle() const
+{
+    return yaw_angle_tracker->GetAngle();
+}
+
+/**
+ * Return the rate of rotation of the yaw (Z-axis) gyro, in degrees per second.
+ *<p>
+ * The rate is based on the most recent reading of the yaw gyro angle.
+ *<p>
+ * @return The current rate of change in yaw angle (in degrees per second)
+ */
+
+double AHRS::GetRate() const
+{
+    return yaw_angle_tracker->GetRate();
+}
+
+/**
+ * Sets an amount of angle to be automatically added before returning a
+ * angle from the getAngle() method.  This allows users of the getAngle() method
+ * to logically rotate the sensor by a given amount of degrees.
+ * <p>
+ * NOTE 1:  The adjustment angle is <b>only</b> applied to the value returned
+ * from getAngle() - it does not adjust the value returned from getYaw(), nor
+ * any of the quaternion values.
+ * <p>
+ * NOTE 2:  The adjustment angle is <b>not</b>automatically cleared whenever the
+ * sensor yaw angle is reset.
+ * <p>
+ * If not set, the default adjustment angle is 0 degrees (no adjustment).
+ * @param adjustment	Adjustment in degrees (range:  -360 to 360)
+ */
+void AHRS::SetAngleAdjustment(double adjustment)
+{
+    yaw_angle_tracker->SetAngleAdjustment(adjustment);
+}
+
+/**
+ * Returns the currently configured adjustment angle.  See
+ * setAngleAdjustment() for more details.
+ *
+ * If this method returns 0 degrees, no adjustment to the value returned
+ * via getAngle() will occur.
+ * @return adjustment, in degrees (range:  -360 to 360)
+ */
+double AHRS::GetAngleAdjustment()
+{
+    return yaw_angle_tracker->GetAngleAdjustment();
+}
+
+/**
+ * Reset the Yaw gyro.
+ *<p>
+ * Resets the Gyro Z (Yaw) axis to a heading of zero. This can be used if
+ * there is significant drift in the gyro and it needs to be recalibrated
+ * after it has been running.
+ */
+void AHRS::Reset()
+{
+    ZeroYaw();
+}
+
+static const float DEV_UNITS_MAX = 32768.0f;
+
+/**
+ * Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec).  NOTE:  this
+ * value is un-processed, and should only be accessed by advanced users.
+ * Typically, rotation about the X Axis is referred to as "Pitch".  Calibrated
+ * and Integrated Pitch data is accessible via the {@link #GetPitch()} method.
+ *<p>
+ * @return Returns the current rotation rate (in degrees/sec).
+ */
+float AHRS::GetRawGyroX()
+{
+    return this->raw_gyro_x / (DEV_UNITS_MAX / (float)gyro_fsr_dps);
+}
+
+/**
+ * Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec).  NOTE:  this
+ * value is un-processed, and should only be accessed by advanced users.
+ * Typically, rotation about the T Axis is referred to as "Roll".  Calibrated
+ * and Integrated Pitch data is accessible via the {@link #GetRoll()} method.
+ *<p>
+ * @return Returns the current rotation rate (in degrees/sec).
+ */
+float AHRS::GetRawGyroY()
+{
+    return this->raw_gyro_y / (DEV_UNITS_MAX / (float)gyro_fsr_dps);
+}
+
+/**
+ * Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec).  NOTE:  this
+ * value is un-processed, and should only be accessed by advanced users.
+ * Typically, rotation about the T Axis is referred to as "Yaw".  Calibrated
+ * and Integrated Pitch data is accessible via the {@link #GetYaw()} method.
+ *<p>
+ * @return Returns the current rotation rate (in degrees/sec).
+ */
+float AHRS::GetRawGyroZ()
+{
+    return this->raw_gyro_z / (DEV_UNITS_MAX / (float)gyro_fsr_dps);
+}
+
+/**
+ * Returns the current raw (unprocessed) X-axis acceleration rate (in G).  NOTE:  this
+ * value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not had acceleration due to gravity removed from it, and has not been rotated to
+ * the world reference frame.  Gravity-corrected, world reference frame-corrected
+ * X axis acceleration data is accessible via the {@link #GetWorldLinearAccelX()} method.
+ *<p>
+ * @return Returns the current acceleration rate (in G).
+ */
+float AHRS::GetRawAccelX()
+{
+    return this->raw_accel_x / (DEV_UNITS_MAX / (float)accel_fsr_g);
+}
+
+/**
+ * Returns the current raw (unprocessed) Y-axis acceleration rate (in G).  NOTE:  this
+ * value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not had acceleration due to gravity removed from it, and has not been rotated to
+ * the world reference frame.  Gravity-corrected, world reference frame-corrected
+ * Y axis acceleration data is accessible via the {@link #GetWorldLinearAccelY()} method.
+ *<p>
+ * @return Returns the current acceleration rate (in G).
+ */
+float AHRS::GetRawAccelY()
+{
+    return this->raw_accel_y / (DEV_UNITS_MAX / (float)accel_fsr_g);
+}
+
+/**
+ * Returns the current raw (unprocessed) Z-axis acceleration rate (in G).  NOTE:  this
+ * value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not had acceleration due to gravity removed from it, and has not been rotated to
+ * the world reference frame.  Gravity-corrected, world reference frame-corrected
+ * Z axis acceleration data is accessible via the {@link #GetWorldLinearAccelZ()} method.
+ *<p>
+ * @return Returns the current acceleration rate (in G).
+ */
+float AHRS::GetRawAccelZ()
+{
+    return this->raw_accel_z / (DEV_UNITS_MAX / (float)accel_fsr_g);
+}
+
+static const float UTESLA_PER_DEV_UNIT = 0.15f;
+
+/**
+ * Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla).  NOTE:
+ * this value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not been tilt-corrected, and has not been combined with the other magnetometer axis
+ * data to yield a compass heading.  Tilt-corrected compass heading data is accessible
+ * via the {@link #GetCompassHeading()} method.
+ *<p>
+ * @return Returns the mag field strength (in uTesla).
+ */
+float AHRS::GetRawMagX()
+{
+    return this->cal_mag_x / UTESLA_PER_DEV_UNIT;
+}
+
+/**
+ * Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla).  NOTE:
+ * this value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not been tilt-corrected, and has not been combined with the other magnetometer axis
+ * data to yield a compass heading.  Tilt-corrected compass heading data is accessible
+ * via the {@link #GetCompassHeading()} method.
+ *<p>
+ * @return Returns the mag field strength (in uTesla).
+ */
+float AHRS::GetRawMagY()
+{
+    return this->cal_mag_y / UTESLA_PER_DEV_UNIT;
+}
+
+/**
+ * Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla).  NOTE:
+ * this value is unprocessed, and should only be accessed by advanced users.  This raw value
+ * has not been tilt-corrected, and has not been combined with the other magnetometer axis
+ * data to yield a compass heading.  Tilt-corrected compass heading data is accessible
+ * via the {@link #GetCompassHeading()} method.
+ *<p>
+ * @return Returns the mag field strength (in uTesla).
+ */
+float AHRS::GetRawMagZ()
+{
+    return this->cal_mag_z / UTESLA_PER_DEV_UNIT;
+}
+
+/**
+ * Returns the current barometric pressure (in millibar) [navX Aero only].
+ *<p>
+ *This value is valid only if a barometric pressure sensor is onboard.
+ *
+ * @return Returns the current barometric pressure (in millibar).
+ */
+float AHRS::GetPressure()
+{
+    // TODO implement for navX-Aero.
+    return 0;
+}
+
+/**
+ * Returns the current temperature (in degrees centigrade) reported by
+ * the sensor's gyro/accelerometer circuit.
+ *<p>
+ * This value may be useful in order to perform advanced temperature-
+ * correction of raw gyroscope and accelerometer values.
+ *<p>
+ * @return The current temperature (in degrees centigrade).
+ */
+float AHRS::GetTempC()
+{
+    return this->mpu_temp_c;
+}
+
+/**
+ * Returns information regarding which sensor board axis (X,Y or Z) and
+ * direction (up/down) is currently configured to report Yaw (Z) angle
+ * values.   NOTE:  If the board firmware supports Omnimount, the board yaw
+ * axis/direction are configurable.
+ *<p>
+ * For more information on Omnimount, please see:
+ *<p>
+ * http://navx-mxp.kauailabs.com/navx-mxp/installation/omnimount/
+ *<p>
+ * @return The currently-configured board yaw axis/direction.
+ */
+AHRS::BoardYawAxis AHRS::GetBoardYawAxis()
+{
+    BoardYawAxis yaw_axis;
+    short yaw_axis_info = (short)(capability_flags >> 3);
+    yaw_axis_info &= 7;
+    if (yaw_axis_info == OMNIMOUNT_DEFAULT)
+    {
+        yaw_axis.up = true;
+        yaw_axis.board_axis = BoardAxis::kBoardAxisZ;
+    }
+    else
+    {
+        yaw_axis.up = (((yaw_axis_info & 0x01) != 0) ? true : false);
+        yaw_axis_info >>= 1;
+        switch (yaw_axis_info)
+        {
+        case 0:
+            yaw_axis.board_axis = BoardAxis::kBoardAxisX;
+            break;
+        case 1:
+            yaw_axis.board_axis = BoardAxis::kBoardAxisY;
+            break;
+        case 2:
+        default:
+            yaw_axis.board_axis = BoardAxis::kBoardAxisZ;
+            break;
+        }
+    }
+    return yaw_axis;
+}
+
+/**
+ * Returns the version number of the firmware currently executing
+ * on the sensor.
+ *<p>
+ * To update the firmware to the latest version, please see:
+ *<p>
+ *   http://navx-mxp.kauailabs.com/navx-mxp/support/updating-firmware/
+ *<p>
+ * @return The firmware version in the format [MajorVersion].[MinorVersion]
+ */
+std::string AHRS::GetFirmwareVersion()
+{
+    std::ostringstream os;
+    os << (int)fw_ver_major << "." << (int)fw_ver_minor;
+    std::string fw_version = os.str();
+    return fw_version;
+}
+
+/***********************************************************/
+/* SendableBase Interface Implementation                   */
+/***********************************************************/
+
+void AHRS::InitSendable(wpi::SendableBuilder &builder)
+{
+    builder.SetSmartDashboardType("Gyro");
+    ;
+    builder.AddDoubleProperty(
+        "Value", [this]()
+        { return GetYaw(); },
+        nullptr);
+}
+
+int AHRS::ThreadFunc(IIOProvider *io_provider)
+{
+    io_provider->Run();
+    return 0;
+}
+
+/**
+ * Registers a callback interface.  This interface
+ * will be called back when new data is available,
+ * based upon a change in the sensor timestamp.
+ *<p>
+ * Note that this callback will occur within the context of the
+ * device IO thread, which is not the same thread context the
+ * caller typically executes in.
+ */
+bool AHRS::RegisterCallback(ITimestampedDataSubscriber *callback, void *callback_context)
+{
+    bool registered = false;
+    for (int i = 0; i < MAX_NUM_CALLBACKS; i++)
+    {
+        if (callbacks[i] == NULL)
+        {
+            callbacks[i] = callback;
+            callback_contexts[i] = callback_context;
+            registered = true;
+            break;
+        }
+    }
+    return registered;
+}
+
+/**
+ * Deregisters a previously registered callback interface.
+ *
+ * Be sure to deregister any callback which have been
+ * previously registered, to ensure that the object
+ * implementing the callback interface does not continue
+ * to be accessed when no longer necessary.
+ */
+bool AHRS::DeregisterCallback(ITimestampedDataSubscriber *callback)
+{
+    bool deregistered = false;
+    for (int i = 0; i < MAX_NUM_CALLBACKS; i++)
+    {
+        if (callbacks[i] == callback)
+        {
+            callbacks[i] = NULL;
+            deregistered = true;
+            break;
+        }
+    }
+    return deregistered;
+}
+
+/**
+ * Returns the navX-Model device's currently configured update
+ * rate.  Note that the update rate that can actually be realized
+ * is a value evenly divisible by the navX-Model device's internal
+ * motion processor sample clock (200Hz).  Therefore, the rate that
+ * is returned may be lower than the requested sample rate.
+ *
+ * The actual sample rate is rounded down to the nearest integer
+ * that is divisible by the number of Digital Motion Processor clock
+ * ticks.  For instance, a request for 58 Hertz will result in
+ * an actual rate of 66Hz (200 / (200 / 58), using integer
+ * math.
+ *
+ * @return Returns the current actual update rate in Hz
+ * (cycles per second).
+ */
+
+int AHRS::GetActualUpdateRate()
+{
+    uint8_t actual_update_rate = GetActualUpdateRateInternal(GetRequestedUpdateRate());
+    return (int)actual_update_rate;
+}
+
+uint8_t AHRS::GetActualUpdateRateInternal(uint8_t update_rate)
+{
+#define NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ 200
+    int integer_update_rate = (int)update_rate;
+    int realized_update_rate = NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ /
+                               (NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ / integer_update_rate);
+    return (uint8_t)realized_update_rate;
+}
+
+/**
+ * Returns the currently requested update rate.
+ * rate.  Note that not every update rate can actually be realized,
+ * since the actual update rate must be a value evenly divisible by
+ * the navX-Model device's internal motion processor sample clock (200Hz).
+ *
+ * To determine the actual update rate, use the
+ * {@link #GetActualUpdateRate()} method.
+ *
+ * @return Returns the requested update rate in Hz
+ * (cycles per second).
+ */
+
+int AHRS::GetRequestedUpdateRate()
+{
+    return (int)update_rate_hz;
+}
+
+// Gyro interface implementation
+void AHRS::Calibrate()
+{
+}
diff --git a/src/main/native/cpp/ContinuousAngleTracker.cpp b/src/main/native/cpp/ContinuousAngleTracker.cpp
new file mode 100644
index 0000000..cbcac4e
--- /dev/null
+++ b/src/main/native/cpp/ContinuousAngleTracker.cpp
@@ -0,0 +1,132 @@
+/*
+ * ContinuousAngleTracker.cpp
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#include "ContinuousAngleTracker.h"
+
+ContinuousAngleTracker::ContinuousAngleTracker()
+{
+	Init();
+	angleAdjust = 0.0f;
+}
+
+void ContinuousAngleTracker::Init()
+{
+	gyro_prevVal = 0.0;
+	ctrRollOver = 0;
+	fFirstUse = true;
+	last_yaw_angle = 0.0f;
+	curr_yaw_angle = 0.0f;
+}
+
+void ContinuousAngleTracker::NextAngle(float newAngle)
+{
+	std::unique_lock<std::mutex> sync(tracker_mutex);
+	last_yaw_angle = curr_yaw_angle;
+	curr_yaw_angle = newAngle;
+}
+
+/* Invoked (internally) whenever yaw reset occurs. */
+void ContinuousAngleTracker::Reset()
+{
+	std::unique_lock<std::mutex> sync(tracker_mutex);
+	Init();
+}
+
+double ContinuousAngleTracker::GetAngle()
+{
+	// First case
+	// Old reading: +150 degrees
+	// New reading: +170 degrees
+	// Difference:  (170 - 150) = +20 degrees
+
+	// Second case
+	// Old reading: -20 degrees
+	// New reading: -50 degrees
+	// Difference : (-50 - -20) = -30 degrees
+
+	// Third case
+	// Old reading: +179 degrees
+	// New reading: -179 degrees
+	// Difference:  (-179 - 179) = -358 degrees
+
+	// Fourth case
+	// Old reading: -179  degrees
+	// New reading: +179 degrees
+	// Difference:  (+179 - -179) = +358 degrees
+
+	double difference;
+	double gyroVal;
+	double yawVal;
+
+	{
+		std::unique_lock<std::mutex> sync(tracker_mutex);
+
+		yawVal = curr_yaw_angle;
+
+		// Has gyro_prevVal been previously set?
+		// If not, return do not calculate, return current value
+		if (!fFirstUse)
+		{
+			// Determine count for rollover counter
+			difference = yawVal - gyro_prevVal;
+
+			/* Clockwise past +180 degrees
+			 * If difference > 180*, increment rollover counter */
+			if (difference < -180.0)
+			{
+				ctrRollOver++;
+
+				/* Counter-clockwise past -180 degrees:
+				 * If difference > 180*, decrement rollover counter */
+			}
+			else if (difference > 180.0)
+			{
+				ctrRollOver--;
+			}
+		}
+
+		// Mark gyro_prevVal as being used
+		fFirstUse = false;
+
+		// Calculate value to return back to calling function
+		// e.g. +720 degrees or -360 degrees
+		gyroVal = yawVal + (360.0 * ctrRollOver);
+		gyro_prevVal = yawVal;
+
+		return gyroVal + angleAdjust;
+	}
+}
+
+void ContinuousAngleTracker::SetAngleAdjustment(double adjustment)
+{
+	angleAdjust = adjustment;
+}
+
+double ContinuousAngleTracker::GetAngleAdjustment()
+{
+	return angleAdjust;
+}
+
+double ContinuousAngleTracker::GetRate()
+{
+	float difference;
+	{
+		std::unique_lock<std::mutex> sync(tracker_mutex);
+		difference = curr_yaw_angle - last_yaw_angle;
+	}
+	if (difference > 180.0f)
+	{
+		/* Clockwise past +180 degrees */
+		difference = 360.0f - difference;
+	}
+	else if (difference < -180.0f)
+	{
+		/* Counter-clockwise past -180 degrees */
+		difference = 360.0f + difference;
+	}
+	return difference;
+}
diff --git a/src/main/native/cpp/ContinuousAngleTracker.h b/src/main/native/cpp/ContinuousAngleTracker.h
new file mode 100644
index 0000000..f7a7177
--- /dev/null
+++ b/src/main/native/cpp/ContinuousAngleTracker.h
@@ -0,0 +1,37 @@
+/*
+ * ContinuousAngleTracker.h
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_CONTINUOUSANGLETRACKER_H_
+#define SRC_CONTINUOUSANGLETRACKER_H_
+
+#include <wpi/priority_mutex.h>
+
+using namespace wpi;
+
+class ContinuousAngleTracker
+{
+private:
+    bool fFirstUse;
+    double gyro_prevVal;
+    int ctrRollOver;
+    float curr_yaw_angle;
+    float last_yaw_angle;
+    double angleAdjust;
+    std::mutex tracker_mutex;
+
+public:
+    ContinuousAngleTracker();
+    void Init();
+    void Reset();
+    void NextAngle(float newAngle);
+    double GetAngle();
+    double GetRate();
+    void SetAngleAdjustment(double adjustment);
+    double GetAngleAdjustment();
+};
+
+#endif /* SRC_CONTINUOUSANGLETRACKER_H_ */
diff --git a/src/main/native/cpp/IBoardCapabilities.h b/src/main/native/cpp/IBoardCapabilities.h
new file mode 100644
index 0000000..dc855f3
--- /dev/null
+++ b/src/main/native/cpp/IBoardCapabilities.h
@@ -0,0 +1,21 @@
+/*
+ * IBoardCapabilities.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_IBOARDCAPABILITIES_H_
+#define SRC_IBOARDCAPABILITIES_H_
+
+class IBoardCapabilities
+{
+public:
+    IBoardCapabilities() {}
+    virtual bool IsOmniMountSupported() = 0;
+    virtual bool IsBoardYawResetSupported() = 0;
+    virtual bool IsDisplacementSupported() = 0;
+    virtual bool IsAHRSPosTimestampSupported() = 0;
+};
+
+#endif /* SRC_IBOARDCAPABILITIES_H_ */
diff --git a/src/main/native/cpp/IIOCompleteNotification.h b/src/main/native/cpp/IIOCompleteNotification.h
new file mode 100644
index 0000000..ad07c43
--- /dev/null
+++ b/src/main/native/cpp/IIOCompleteNotification.h
@@ -0,0 +1,42 @@
+/*
+ * IIOCompleteNotification.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_IIOCOMPLETENOTIFICATION_H_
+#define SRC_IIOCOMPLETENOTIFICATION_H_
+
+#include "IMUProtocol.h"
+#include "AHRSProtocol.h"
+
+#include <stdint.h>
+
+class IIOCompleteNotification
+{
+public:
+    IIOCompleteNotification() {}
+    struct BoardState
+    {
+        uint8_t op_status;
+        int16_t sensor_status;
+        uint8_t cal_status;
+        uint8_t selftest_status;
+        int16_t capability_flags;
+        uint8_t update_rate_hz;
+        int16_t accel_fsr_g;
+        int16_t gyro_fsr_dps;
+    };
+    virtual void SetYawPitchRoll(IMUProtocol::YPRUpdate &ypr_update, long sensor_timestamp) = 0;
+    virtual void SetAHRSData(AHRSProtocol::AHRSUpdate &ahrs_update, long sensor_timestamp) = 0;
+    virtual void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate &ahrs_update, long sensor_timestamp) = 0;
+    virtual void SetRawData(IMUProtocol::GyroUpdate &raw_data_update, long sensor_timestamp) = 0;
+    virtual void SetBoardID(AHRSProtocol::BoardID &board_id) = 0;
+    virtual void SetBoardState(BoardState &board_state, bool update_board_status) = 0;
+    virtual void YawResetComplete() = 0;
+    virtual void DisconnectDetected() = 0;
+    virtual void ConnectDetected() = 0;
+};
+
+#endif /* SRC_IIOCOMPLETENOTIFICATION_H_ */
diff --git a/src/main/native/cpp/IIOProvider.h b/src/main/native/cpp/IIOProvider.h
new file mode 100644
index 0000000..ea4d74c
--- /dev/null
+++ b/src/main/native/cpp/IIOProvider.h
@@ -0,0 +1,28 @@
+/*
+ * IIOProvider.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_IIOPROVIDER_H_
+#define SRC_IIOPROVIDER_H_
+
+#include <stdint.h>
+
+class IIOProvider
+{
+public:
+    IIOProvider() {}
+    virtual bool IsConnected() = 0;
+    virtual double GetByteCount() = 0;
+    virtual double GetUpdateCount() = 0;
+    virtual void SetUpdateRateHz(uint8_t update_rate) = 0;
+    virtual void ZeroYaw() = 0;
+    virtual void ZeroDisplacement() = 0;
+    virtual void Run() = 0;
+    virtual void Stop() = 0;
+    virtual void EnableLogging(bool enable) = 0;
+};
+
+#endif /* SRC_IIOPROVIDER_H_ */
diff --git a/src/main/native/cpp/IRegisterIO.h b/src/main/native/cpp/IRegisterIO.h
new file mode 100644
index 0000000..1025d49
--- /dev/null
+++ b/src/main/native/cpp/IRegisterIO.h
@@ -0,0 +1,24 @@
+/*
+ * IRegisterIO.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_IREGISTERIO_H_
+#define SRC_IREGISTERIO_H_
+
+#include <stdint.h>
+
+class IRegisterIO
+{
+public:
+    IRegisterIO() {}
+    virtual bool Init() = 0;
+    virtual bool Write(uint8_t address, uint8_t value) = 0;
+    virtual bool Read(uint8_t first_address, uint8_t *buffer, uint8_t buffer_len) = 0;
+    virtual bool Shutdown() = 0;
+    virtual void EnableLogging(bool enable) = 0;
+};
+
+#endif /* SRC_IREGISTERIO_H_ */
diff --git a/src/main/native/cpp/InertialDataIntegrator.cpp b/src/main/native/cpp/InertialDataIntegrator.cpp
new file mode 100644
index 0000000..6bbac47
--- /dev/null
+++ b/src/main/native/cpp/InertialDataIntegrator.cpp
@@ -0,0 +1,78 @@
+/*
+ * InertialDataIntegrator.cpp
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#include "InertialDataIntegrator.h"
+
+InertialDataIntegrator::InertialDataIntegrator()
+{
+    ResetDisplacement();
+}
+
+void InertialDataIntegrator::UpdateDisplacement(float accel_x_g, float accel_y_g,
+                                                int update_rate_hz, bool is_moving)
+{
+    if (is_moving)
+    {
+        float accel_g[2];
+        float accel_m_s2[2];
+        float curr_velocity_m_s[2];
+        float sample_time = (1.0f / update_rate_hz);
+        accel_g[0] = accel_x_g;
+        accel_g[1] = accel_y_g;
+        for (int i = 0; i < 2; i++)
+        {
+            accel_m_s2[i] = accel_g[i] * 9.80665f;
+            curr_velocity_m_s[i] = last_velocity[i] + (accel_m_s2[i] * sample_time);
+            displacement[i] += last_velocity[i] + (0.5f * accel_m_s2[i] * sample_time * sample_time);
+            last_velocity[i] = curr_velocity_m_s[i];
+        }
+    }
+    else
+    {
+        last_velocity[0] = 0.0f;
+        last_velocity[1] = 0.0f;
+    }
+}
+
+void InertialDataIntegrator::ResetDisplacement()
+{
+    for (int i = 0; i < 2; i++)
+    {
+        last_velocity[i] = 0.0f;
+        displacement[i] = 0.0f;
+    }
+}
+
+float InertialDataIntegrator::GetVelocityX()
+{
+    return last_velocity[0];
+}
+
+float InertialDataIntegrator::GetVelocityY()
+{
+    return last_velocity[1];
+}
+
+float InertialDataIntegrator::GetVelocityZ()
+{
+    return 0;
+}
+
+float InertialDataIntegrator::GetDisplacementX()
+{
+    return displacement[0];
+}
+
+float InertialDataIntegrator::GetDisplacementY()
+{
+    return displacement[1];
+}
+
+float InertialDataIntegrator::GetDisplacementZ()
+{
+    return 0;
+}
diff --git a/src/main/native/cpp/InertialDataIntegrator.h b/src/main/native/cpp/InertialDataIntegrator.h
new file mode 100644
index 0000000..250335b
--- /dev/null
+++ b/src/main/native/cpp/InertialDataIntegrator.h
@@ -0,0 +1,29 @@
+/*
+ * InertialDataIntegrator.h
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_INERTIALDATAINTEGRATOR_H_
+#define SRC_INERTIALDATAINTEGRATOR_H_
+
+class InertialDataIntegrator
+{
+    float last_velocity[2];
+    float displacement[2];
+
+public:
+    InertialDataIntegrator();
+    void UpdateDisplacement(float accel_x_g, float accel_y_g,
+                            int update_rate_hz, bool is_moving);
+    void ResetDisplacement();
+    float GetVelocityX();
+    float GetVelocityY();
+    float GetVelocityZ();
+    float GetDisplacementX();
+    float GetDisplacementY();
+    float GetDisplacementZ();
+};
+
+#endif /* SRC_INERTIALDATAINTEGRATOR_H_ */
diff --git a/src/main/native/cpp/OffsetTracker.cpp b/src/main/native/cpp/OffsetTracker.cpp
new file mode 100644
index 0000000..50ae88a
--- /dev/null
+++ b/src/main/native/cpp/OffsetTracker.cpp
@@ -0,0 +1,65 @@
+/*
+ * OffsetTracker.cpp
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#include "OffsetTracker.h"
+
+OffsetTracker::OffsetTracker(int history_length)
+{
+    history_len = history_length;
+    value_history = new float[history_len];
+    for (int i = 0; i < history_len; i++)
+    {
+        value_history[i] = 0.0f;
+    }
+    next_value_history_index = 0;
+    value_offset = 0;
+}
+
+void OffsetTracker::UpdateHistory(float curr_value)
+{
+    if (next_value_history_index >= history_len)
+    {
+        next_value_history_index = 0;
+    }
+    value_history[next_value_history_index] = curr_value;
+    next_value_history_index++;
+}
+
+double OffsetTracker::GetAverageFromHistory()
+{
+    double value_history_sum = 0.0;
+    for (int i = 0; i < history_len; i++)
+    {
+        value_history_sum += value_history[i];
+    }
+    double value_history_avg = value_history_sum / history_len;
+    return value_history_avg;
+}
+
+void OffsetTracker::SetOffset()
+{
+    value_offset = GetAverageFromHistory();
+}
+
+double OffsetTracker::GetOffset()
+{
+    return value_offset;
+}
+
+double OffsetTracker::ApplyOffset(double value)
+{
+    float offseted_value = (float)(value - value_offset);
+    if (offseted_value < -180)
+    {
+        offseted_value += 360;
+    }
+    if (offseted_value > 180)
+    {
+        offseted_value -= 360;
+    }
+    return offseted_value;
+}
diff --git a/src/main/native/cpp/OffsetTracker.h b/src/main/native/cpp/OffsetTracker.h
new file mode 100644
index 0000000..c885317
--- /dev/null
+++ b/src/main/native/cpp/OffsetTracker.h
@@ -0,0 +1,29 @@
+/*
+ * OffsetTracker.h
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_OFFSETTRACKER_H_
+#define SRC_OFFSETTRACKER_H_
+
+class OffsetTracker
+{
+    float *value_history;
+    int next_value_history_index;
+    int history_len;
+    double value_offset;
+
+public:
+    OffsetTracker(int history_length);
+    void UpdateHistory(float curr_value);
+    void SetOffset();
+    double ApplyOffset(double value);
+
+private:
+    double GetAverageFromHistory();
+    double GetOffset();
+};
+
+#endif /* SRC_OFFSETTRACKER_H_ */
diff --git a/src/main/native/cpp/RegisterIO.cpp b/src/main/native/cpp/RegisterIO.cpp
new file mode 100644
index 0000000..2259512
--- /dev/null
+++ b/src/main/native/cpp/RegisterIO.cpp
@@ -0,0 +1,274 @@
+/*
+ * RegisterIO.cpp
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#include "RegisterIO.h"
+#include "IMURegisters.h"
+#include "delay.h"
+#include "Tracer.h"
+
+#include <units/base.h>
+
+using namespace frc;
+using units::unit_cast;
+
+RegisterIO::RegisterIO(IRegisterIO *io_provider,
+                       uint8_t update_rate_hz,
+                       IIOCompleteNotification *notify_sink,
+                       IBoardCapabilities *board_capabilities)
+{
+    this->io_provider = io_provider;
+    this->update_rate_hz = update_rate_hz;
+    this->board_capabilities = board_capabilities;
+    this->notify_sink = notify_sink;
+    this->last_sensor_timestamp = 0;
+    this->update_count = 0;
+    this->byte_count = 0;
+    this->last_update_time = 0;
+    this->stop = false;
+    this->disconnect_reported = false;
+    this->connect_reported = false;
+
+    raw_data_update = {};
+    ahrs_update = {};
+    ahrspos_update = {};
+    board_state = {};
+    board_id = {};
+}
+
+static const double IO_TIMEOUT_SECONDS = 1.0;
+static const double DELAY_OVERHEAD_MILLISECONDS = 4.0;
+
+RegisterIO::~RegisterIO()
+{
+}
+
+bool RegisterIO::IsConnected()
+{
+    double time_since_last_update = unit_cast<double>(Timer::GetFPGATimestamp()) - this->last_update_time;
+    return time_since_last_update <= IO_TIMEOUT_SECONDS;
+}
+
+double RegisterIO::GetByteCount()
+{
+    return byte_count;
+}
+
+double RegisterIO::GetUpdateCount()
+{
+    return update_count;
+}
+
+void RegisterIO::SetUpdateRateHz(uint8_t update_rate)
+{
+    io_provider->Write(NAVX_REG_UPDATE_RATE_HZ, update_rate);
+}
+
+void RegisterIO::ZeroYaw()
+{
+    io_provider->Write(NAVX_REG_INTEGRATION_CTL,
+                       NAVX_INTEGRATION_CTL_RESET_YAW);
+    notify_sink->YawResetComplete();
+}
+
+void RegisterIO::ZeroDisplacement()
+{
+    io_provider->Write(NAVX_REG_INTEGRATION_CTL,
+                       NAVX_INTEGRATION_CTL_RESET_DISP_X |
+                           NAVX_INTEGRATION_CTL_RESET_DISP_Y |
+                           NAVX_INTEGRATION_CTL_RESET_DISP_Z);
+}
+
+void RegisterIO::Run()
+{
+    io_provider->Init();
+
+    /* Initial Device Configuration */
+    SetUpdateRateHz(this->update_rate_hz);
+    GetConfiguration();
+
+    double update_rate_ms = 1.0 / (double)this->update_rate_hz;
+    if (update_rate_ms > DELAY_OVERHEAD_MILLISECONDS)
+    {
+        update_rate_ms -= DELAY_OVERHEAD_MILLISECONDS;
+    }
+
+    /* IO Loop */
+    while (!stop)
+    {
+        if (board_state.update_rate_hz != this->update_rate_hz)
+        {
+            SetUpdateRateHz(this->update_rate_hz);
+        }
+        GetCurrentData();
+        delayMillis(update_rate_ms);
+    }
+}
+
+void RegisterIO::Stop()
+{
+    stop = true;
+}
+
+void RegisterIO::EnableLogging(bool enable)
+{
+    io_provider->EnableLogging(enable);
+}
+
+bool RegisterIO::GetConfiguration()
+{
+    bool success = false;
+    int retry_count = 0;
+    while (retry_count < 3 && !success)
+    {
+        char config[NAVX_REG_SENSOR_STATUS_H + 1] = {0};
+        if (io_provider->Read(NAVX_REG_WHOAMI, (uint8_t *)config, sizeof(config)) &&
+            (config[NAVX_REG_WHOAMI] == NAVX_MODEL_NAVX_MXP))
+        {
+
+            if (!connect_reported)
+            {
+                notify_sink->ConnectDetected();
+                connect_reported = true;
+                disconnect_reported = false;
+            }
+
+            board_id.hw_rev = config[NAVX_REG_HW_REV];
+            board_id.fw_ver_major = config[NAVX_REG_FW_VER_MAJOR];
+            board_id.fw_ver_minor = config[NAVX_REG_FW_VER_MINOR];
+            board_id.type = config[NAVX_REG_WHOAMI];
+
+            notify_sink->SetBoardID(board_id);
+
+            board_state.cal_status = config[NAVX_REG_CAL_STATUS];
+            board_state.op_status = config[NAVX_REG_OP_STATUS];
+            board_state.selftest_status = config[NAVX_REG_SELFTEST_STATUS];
+            board_state.sensor_status = IMURegisters::decodeProtocolUint16(config + NAVX_REG_SENSOR_STATUS_L);
+            board_state.gyro_fsr_dps = IMURegisters::decodeProtocolUint16(config + NAVX_REG_GYRO_FSR_DPS_L);
+            board_state.accel_fsr_g = (int16_t)config[NAVX_REG_ACCEL_FSR_G];
+            board_state.update_rate_hz = config[NAVX_REG_UPDATE_RATE_HZ];
+            board_state.capability_flags = IMURegisters::decodeProtocolUint16(config + NAVX_REG_CAPABILITY_FLAGS_L);
+            bool update_board_status = true;
+            notify_sink->SetBoardState(board_state, update_board_status);
+            success = true;
+        }
+        else
+        {
+            success = false;
+            delayMillis(50);
+        }
+        retry_count++;
+    }
+    return success;
+}
+
+void RegisterIO::GetCurrentData()
+{
+    uint8_t first_address = NAVX_REG_UPDATE_RATE_HZ;
+    bool displacement_registers = board_capabilities->IsDisplacementSupported();
+    uint8_t buffer_len;
+    char curr_data[NAVX_REG_LAST + 1];
+    /* If firmware supports displacement data, acquire it - otherwise implement */
+    /* similar (but potentially less accurate) calculations on this processor.  */
+    if (displacement_registers)
+    {
+        buffer_len = NAVX_REG_LAST + 1 - first_address;
+    }
+    else
+    {
+        buffer_len = NAVX_REG_HIRES_TIMESTAMP_H_H_H + 1 - first_address;
+    }
+    if (io_provider->Read(first_address, (uint8_t *)curr_data, buffer_len))
+    {
+        long sensor_timestamp = IMURegisters::decodeProtocolUint32(curr_data + NAVX_REG_TIMESTAMP_L_L - first_address);
+        if (!connect_reported)
+        {
+            notify_sink->ConnectDetected();
+            connect_reported = true;
+            disconnect_reported = false;
+        }
+        if (sensor_timestamp == last_sensor_timestamp)
+        {
+            return;
+        }
+        last_sensor_timestamp = sensor_timestamp;
+        ahrspos_update.op_status = curr_data[NAVX_REG_OP_STATUS - first_address];
+        ahrspos_update.selftest_status = curr_data[NAVX_REG_SELFTEST_STATUS - first_address];
+        ahrspos_update.cal_status = curr_data[NAVX_REG_CAL_STATUS - first_address];
+        ahrspos_update.sensor_status = curr_data[NAVX_REG_SENSOR_STATUS_L - first_address];
+        ahrspos_update.yaw = IMURegisters::decodeProtocolSignedHundredthsFloat(curr_data + NAVX_REG_YAW_L - first_address);
+        ahrspos_update.pitch = IMURegisters::decodeProtocolSignedHundredthsFloat(curr_data + NAVX_REG_PITCH_L - first_address);
+        ahrspos_update.roll = IMURegisters::decodeProtocolSignedHundredthsFloat(curr_data + NAVX_REG_ROLL_L - first_address);
+        ahrspos_update.compass_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(curr_data + NAVX_REG_HEADING_L - first_address);
+        ahrspos_update.mpu_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(curr_data + NAVX_REG_MPU_TEMP_C_L - first_address);
+        ahrspos_update.linear_accel_x = IMURegisters::decodeProtocolSignedThousandthsFloat(curr_data + NAVX_REG_LINEAR_ACC_X_L - first_address);
+        ahrspos_update.linear_accel_y = IMURegisters::decodeProtocolSignedThousandthsFloat(curr_data + NAVX_REG_LINEAR_ACC_Y_L - first_address);
+        ahrspos_update.linear_accel_z = IMURegisters::decodeProtocolSignedThousandthsFloat(curr_data + NAVX_REG_LINEAR_ACC_Z_L - first_address);
+        ahrspos_update.altitude = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_ALTITUDE_D_L - first_address);
+        ahrspos_update.barometric_pressure = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_PRESSURE_DL - first_address);
+        ahrspos_update.fused_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(curr_data + NAVX_REG_FUSED_HEADING_L - first_address);
+        ahrspos_update.quat_w = ((float)IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_QUAT_W_L - first_address)) / 32768.0f;
+        ahrspos_update.quat_x = ((float)IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_QUAT_X_L - first_address)) / 32768.0f;
+        ahrspos_update.quat_y = ((float)IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_QUAT_Y_L - first_address)) / 32768.0f;
+        ahrspos_update.quat_z = ((float)IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_QUAT_Z_L - first_address)) / 32768.0f;
+        if (displacement_registers)
+        {
+            ahrspos_update.vel_x = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_VEL_X_I_L - first_address);
+            ahrspos_update.vel_y = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_VEL_Y_I_L - first_address);
+            ahrspos_update.vel_z = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_VEL_Z_I_L - first_address);
+            ahrspos_update.disp_x = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_DISP_X_I_L - first_address);
+            ahrspos_update.disp_y = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_DISP_Y_I_L - first_address);
+            ahrspos_update.disp_z = IMURegisters::decodeProtocol1616Float(curr_data + NAVX_REG_DISP_Z_I_L - first_address);
+            notify_sink->SetAHRSPosData(ahrspos_update, sensor_timestamp);
+        }
+        else
+        {
+            ahrs_update.op_status = ahrspos_update.op_status;
+            ahrs_update.selftest_status = ahrspos_update.selftest_status;
+            ahrs_update.cal_status = ahrspos_update.cal_status;
+            ahrs_update.sensor_status = ahrspos_update.sensor_status;
+            ahrs_update.yaw = ahrspos_update.yaw;
+            ahrs_update.pitch = ahrspos_update.pitch;
+            ahrs_update.roll = ahrspos_update.roll;
+            ahrs_update.compass_heading = ahrspos_update.compass_heading;
+            ahrs_update.mpu_temp = ahrspos_update.mpu_temp;
+            ahrs_update.linear_accel_x = ahrspos_update.linear_accel_x;
+            ahrs_update.linear_accel_y = ahrspos_update.linear_accel_y;
+            ahrs_update.linear_accel_z = ahrspos_update.linear_accel_z;
+            ahrs_update.altitude = ahrspos_update.altitude;
+            ahrs_update.barometric_pressure = ahrspos_update.barometric_pressure;
+            ahrs_update.fused_heading = ahrspos_update.fused_heading;
+            notify_sink->SetAHRSData(ahrs_update, sensor_timestamp);
+        }
+
+        board_state.update_rate_hz = curr_data[NAVX_REG_UPDATE_RATE_HZ - first_address];
+        board_state.gyro_fsr_dps = IMURegisters::decodeProtocolUint16(curr_data + NAVX_REG_GYRO_FSR_DPS_L - first_address);
+        board_state.accel_fsr_g = (int16_t)curr_data[NAVX_REG_ACCEL_FSR_G - first_address];
+        board_state.capability_flags = IMURegisters::decodeProtocolUint16(curr_data + NAVX_REG_CAPABILITY_FLAGS_L - first_address);
+        bool update_board_status = false;
+        notify_sink->SetBoardState(board_state, update_board_status); // Board status already updated previously above
+
+        raw_data_update.gyro_x = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_GYRO_X_L - first_address);
+        raw_data_update.gyro_y = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_GYRO_Y_L - first_address);
+        raw_data_update.gyro_z = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_GYRO_Z_L - first_address);
+        raw_data_update.accel_x = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_ACC_X_L - first_address);
+        raw_data_update.accel_y = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_ACC_Y_L - first_address);
+        raw_data_update.accel_z = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_ACC_Z_L - first_address);
+        raw_data_update.mag_x = IMURegisters::decodeProtocolInt16(curr_data + NAVX_REG_MAG_X_L - first_address);
+        raw_data_update.temp_c = ahrspos_update.mpu_temp;
+        notify_sink->SetRawData(raw_data_update, sensor_timestamp);
+
+        this->last_update_time = unit_cast<double>(Timer::GetFPGATimestamp());
+        byte_count += buffer_len;
+        update_count++;
+    }
+    if (connect_reported && !disconnect_reported && !this->IsConnected())
+    {
+        notify_sink->DisconnectDetected();
+        disconnect_reported = true;
+        connect_reported = false;
+    }
+}
diff --git a/src/main/native/cpp/RegisterIO.h b/src/main/native/cpp/RegisterIO.h
new file mode 100644
index 0000000..555bfa4
--- /dev/null
+++ b/src/main/native/cpp/RegisterIO.h
@@ -0,0 +1,63 @@
+/*
+ * RegisterIO.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_REGISTERIO_H_
+#define SRC_REGISTERIO_H_
+
+#include "IIOProvider.h"
+#include "IRegisterIO.h"
+#include "IMUProtocol.h"
+#include "AHRSProtocol.h"
+#include "IBoardCapabilities.h"
+#include "IIOCompleteNotification.h"
+
+#include <frc/Timer.h>
+
+#include <stdint.h>
+
+class RegisterIO : public IIOProvider
+{
+private:
+    IRegisterIO *io_provider;
+    uint8_t update_rate_hz;
+    bool stop;
+    IMUProtocol::GyroUpdate raw_data_update;
+    AHRSProtocol::AHRSUpdate ahrs_update;
+    AHRSProtocol::AHRSPosUpdate ahrspos_update;
+    IIOCompleteNotification *notify_sink;
+    IIOCompleteNotification::BoardState board_state;
+    AHRSProtocol::BoardID board_id;
+    IBoardCapabilities *board_capabilities;
+    double last_update_time;
+    int byte_count;
+    int update_count;
+    long last_sensor_timestamp;
+    bool disconnect_reported;
+    bool connect_reported;
+
+public:
+    RegisterIO(IRegisterIO *io_provider,
+               uint8_t update_rate_hz,
+               IIOCompleteNotification *notify_sink,
+               IBoardCapabilities *board_capabilities);
+    bool IsConnected();
+    double GetByteCount();
+    double GetUpdateCount();
+    void SetUpdateRateHz(uint8_t update_rate);
+    void ZeroYaw();
+    void ZeroDisplacement();
+    void Run();
+    void Stop();
+    void EnableLogging(bool enable);
+    virtual ~RegisterIO();
+
+private:
+    bool GetConfiguration();
+    void GetCurrentData();
+};
+
+#endif /* SRC_REGISTERIO_H_ */
diff --git a/src/main/native/cpp/RegisterIOI2C.cpp b/src/main/native/cpp/RegisterIOI2C.cpp
new file mode 100644
index 0000000..831a449
--- /dev/null
+++ b/src/main/native/cpp/RegisterIOI2C.cpp
@@ -0,0 +1,87 @@
+/*
+ * RegisterIOI2C.cpp
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#include "RegisterIOI2C.h"
+#include "Tracer.h"
+
+#include <frc/Timer.h>
+#include <wpi/mutex.h>
+
+#include <cstring>
+
+using namespace wpi;
+
+#define NUM_IGNORED_SUCCESSIVE_ERRORS 50
+
+static wpi::mutex imu_mutex;
+RegisterIO_I2C::RegisterIO_I2C(I2C *port)
+{
+    this->port = port;
+    this->trace = false;
+    successive_error_count = 0;
+}
+
+bool RegisterIO_I2C::Init()
+{
+    return true;
+}
+
+bool RegisterIO_I2C::Write(uint8_t address, uint8_t value)
+{
+    std::unique_lock<wpi::mutex> sync(imu_mutex);
+    bool aborted = port->Write(address | 0x80, value);
+    if (aborted && trace)
+        Tracer::Trace("navX-MXP I2C Write error\n");
+    return !aborted;
+}
+
+static const int MAX_WPILIB_I2C_READ_BYTES = 127;
+
+bool RegisterIO_I2C::Read(uint8_t first_address, uint8_t *buffer, uint8_t buffer_len)
+{
+    std::unique_lock<wpi::mutex> sync(imu_mutex);
+    int len = buffer_len;
+    int buffer_offset = 0;
+    uint8_t read_buffer[MAX_WPILIB_I2C_READ_BYTES];
+    while (len > 0)
+    {
+        int read_len = (len > MAX_WPILIB_I2C_READ_BYTES) ? MAX_WPILIB_I2C_READ_BYTES : len;
+        if (!port->Write(first_address + buffer_offset, read_len) &&
+            !port->ReadOnly(read_len, read_buffer))
+        {
+            std::memcpy(buffer + buffer_offset, read_buffer, read_len);
+            buffer_offset += read_len;
+            len -= read_len;
+            successive_error_count = 0;
+        }
+        else
+        {
+            successive_error_count++;
+            if (successive_error_count % NUM_IGNORED_SUCCESSIVE_ERRORS == 1)
+            {
+                if (trace)
+                {
+                    Tracer::Trace("navX-MXP I2C Read error %s.\n",
+                                  ((successive_error_count < NUM_IGNORED_SUCCESSIVE_ERRORS) ? "" : " (Repeated errors omitted)"));
+                }
+                break;
+            }
+            return false;
+        }
+    }
+    return (len == 0);
+}
+
+bool RegisterIO_I2C::Shutdown()
+{
+    return true;
+}
+
+void RegisterIO_I2C::EnableLogging(bool enable)
+{
+    trace = enable;
+}
diff --git a/src/main/native/cpp/RegisterIOI2C.h b/src/main/native/cpp/RegisterIOI2C.h
new file mode 100644
index 0000000..59fd312
--- /dev/null
+++ b/src/main/native/cpp/RegisterIOI2C.h
@@ -0,0 +1,34 @@
+/*
+ * RegisterIOI2C.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_REGISTERIOI2C_H_
+#define SRC_REGISTERIOI2C_H_
+
+#include "RegisterIO.h"
+
+#include <frc/I2C.h>
+
+using namespace frc;
+
+class RegisterIO_I2C : public IRegisterIO
+{
+public:
+    RegisterIO_I2C(I2C *port);
+    virtual ~RegisterIO_I2C() {}
+    bool Init();
+    bool Write(uint8_t address, uint8_t value);
+    bool Read(uint8_t first_address, uint8_t *buffer, uint8_t buffer_len);
+    bool Shutdown();
+    void EnableLogging(bool enable);
+
+private:
+    I2C *port;
+    bool trace;
+    int successive_error_count;
+};
+
+#endif /* SRC_REGISTERIOI2C_H_ */
diff --git a/src/main/native/cpp/RegisterIOSPI.cpp b/src/main/native/cpp/RegisterIOSPI.cpp
new file mode 100644
index 0000000..a80e199
--- /dev/null
+++ b/src/main/native/cpp/RegisterIOSPI.cpp
@@ -0,0 +1,110 @@
+/*
+ * RegisterIOSPI.cpp
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#include "RegisterIOSPI.h"
+#include "Tracer.h"
+
+#include <wpi/mutex.h>
+#include <frc/Timer.h>
+
+#include <cstring>
+
+using namespace wpi;
+
+#define NUM_IGNORED_SUCCESSIVE_ERRORS 50
+
+static wpi::mutex imu_mutex;
+RegisterIO_SPI::RegisterIO_SPI(SPI *port, uint32_t bitrate)
+{
+    this->port = port;
+    this->bitrate = bitrate;
+    this->trace = false;
+    this->successive_error_count = 0;
+}
+
+bool RegisterIO_SPI::Init()
+{
+    port->SetClockRate(bitrate);
+    port->SetMode(frc::SPI::kMode3);
+    port->SetChipSelectActiveLow();
+    if (trace)
+        Tracer::Trace("navX-MXP:  Initialized SPI communication at bitrate %d\n", bitrate);
+    return true;
+}
+
+bool RegisterIO_SPI::Write(uint8_t address, uint8_t value)
+{
+    std::unique_lock<wpi::mutex> sync(imu_mutex);
+    uint8_t cmd[3];
+    cmd[0] = address | 0x80;
+    cmd[1] = value;
+    cmd[2] = IMURegisters::getCRC(cmd, 2);
+    if (port->Write(cmd, sizeof(cmd)) != sizeof(cmd))
+    {
+        if (trace)
+            Tracer::Trace("navX-MXP SPI Write error\n");
+        return false; // WRITE ERROR
+    }
+    return true;
+}
+
+bool RegisterIO_SPI::Read(uint8_t first_address, uint8_t *buffer, uint8_t buffer_len)
+{
+    std::unique_lock<wpi::mutex> sync(imu_mutex);
+    uint8_t cmd[3];
+    cmd[0] = first_address;
+    cmd[1] = buffer_len;
+    cmd[2] = IMURegisters::getCRC(cmd, 2);
+    if (port->Write(cmd, sizeof(cmd)) != sizeof(cmd))
+    {
+        return false; // WRITE ERROR
+    }
+    // delay 200 us /* TODO:  What is min. granularity of delay()? */
+    Wait(units::second_t(0.001));
+    memset(rx_buffer, 0x95, buffer_len - 1);
+    rx_buffer[buffer_len - 1] = 0x3E;
+    if (port->Read(true, rx_buffer, buffer_len + 1) != buffer_len + 1)
+    {
+        if (trace)
+            Tracer::Trace("navX-MXP SPI Read error\n");
+        return false; // READ ERROR
+    }
+    uint8_t crc = IMURegisters::getCRC(rx_buffer, buffer_len);
+    if ((crc != rx_buffer[buffer_len]) ||
+        ((rx_buffer[0] == 0x00) &&
+         (rx_buffer[1] == 0x00) &&
+         (rx_buffer[2] == 0x00) &&
+         (rx_buffer[3] == 0x00)))
+    {
+        successive_error_count++;
+        if (successive_error_count % NUM_IGNORED_SUCCESSIVE_ERRORS == 1)
+        {
+            if (trace)
+            {
+                Tracer::Trace("navX-MXP SPI CRC err.  Length:  %d, Got:  %d; Calculated:  %d %s\n", buffer_len, rx_buffer[buffer_len], crc,
+                              ((successive_error_count < NUM_IGNORED_SUCCESSIVE_ERRORS) ? "" : " (Repeated errors omitted)"));
+            }
+        }
+        return false; // CRC ERROR
+    }
+    else
+    {
+        std::memcpy(buffer, rx_buffer, buffer_len);
+        successive_error_count = 0;
+    }
+    return true;
+}
+
+bool RegisterIO_SPI::Shutdown()
+{
+    return true;
+}
+
+void RegisterIO_SPI::EnableLogging(bool enable)
+{
+    trace = enable;
+}
diff --git a/src/main/native/cpp/RegisterIOSPI.h b/src/main/native/cpp/RegisterIOSPI.h
new file mode 100644
index 0000000..a5ac24e
--- /dev/null
+++ b/src/main/native/cpp/RegisterIOSPI.h
@@ -0,0 +1,38 @@
+/*
+ * RegisterIOSPI.h
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_REGISTERIOSPI_H_
+#define SRC_REGISTERIOSPI_H_
+
+#include "RegisterIO.h"
+
+#include <frc/SPI.h>
+
+using namespace frc;
+
+static const int MAX_SPI_MSG_LENGTH = 256;
+
+class RegisterIO_SPI : public IRegisterIO
+{
+public:
+    RegisterIO_SPI(SPI *port, uint32_t bitrate);
+    virtual ~RegisterIO_SPI() {}
+    bool Init();
+    bool Write(uint8_t address, uint8_t value);
+    bool Read(uint8_t first_address, uint8_t *buffer, uint8_t buffer_len);
+    bool Shutdown();
+    void EnableLogging(bool enable);
+
+private:
+    SPI *port;
+    uint32_t bitrate;
+    uint8_t rx_buffer[MAX_SPI_MSG_LENGTH];
+    bool trace;
+    int successive_error_count;
+};
+
+#endif /* SRC_REGISTERIOSPI_H_ */
diff --git a/src/main/native/cpp/SerialIO.cpp b/src/main/native/cpp/SerialIO.cpp
new file mode 100644
index 0000000..403c330
--- /dev/null
+++ b/src/main/native/cpp/SerialIO.cpp
@@ -0,0 +1,762 @@
+/*
+ * SerialIO.cpp
+ *
+ *  Created on: Jul 31, 2015
+ *      Author: Scott
+ */
+
+#include "SerialIO.h"
+#include "delay.h"
+#include "Tracer.h"
+
+#include <frc/Timer.h>
+#include <units/base.h>
+#include <units/time.h>
+
+#include <cstring>
+
+static const double IO_TIMEOUT_SECONDS = 1.0;
+
+#define SERIALIO_DASHBOARD_DEBUG
+
+using units::unit_cast;
+
+SerialIO::SerialIO(SerialPort::Port port_id,
+                   uint8_t update_rate_hz,
+                   bool processed_data,
+                   IIOCompleteNotification *notify_sink,
+                   IBoardCapabilities *board_capabilities)
+{
+    this->serial_port_id = port_id;
+    is_usb = ((port_id != SerialPort::Port::kMXP) &&
+              (port_id != SerialPort::Port::kOnboard));
+    ypr_update_data = {};
+    gyro_update_data = {};
+    ahrs_update_data = {};
+    ahrspos_update_data = {};
+    ahrspos_ts_update_data = {};
+    board_id = {};
+    board_state = {};
+    this->notify_sink = notify_sink;
+    this->board_capabilities = board_capabilities;
+    serial_port = 0;
+    serial_port = GetMaybeCreateSerialPort();
+    this->update_rate_hz = update_rate_hz;
+    if (processed_data)
+    {
+        update_type = MSGID_AHRSPOS_TS_UPDATE;
+    }
+    else
+    {
+        update_type = MSGID_GYRO_UPDATE;
+    }
+    signal_transmit_integration_control = false;
+    signal_retransmit_stream_config = false;
+    stop = true;
+    byte_count = 0;
+    update_count = 0;
+    last_valid_packet_time = 0;
+    connect_reported =
+        disconnect_reported = false;
+    debug = false;
+}
+
+SerialPort *SerialIO::ResetSerialPort()
+{
+    if (serial_port != 0)
+    {
+        if (connect_reported && !disconnect_reported && !IsConnected())
+        {
+            notify_sink->DisconnectDetected();
+            connect_reported = false;
+            disconnect_reported = true;
+        }
+        Tracer::Trace("Closing %s serial port to communicate with navX-MXP/Micro.\n", (is_usb ? "USB " : "TTL UART "));
+        try
+        {
+            delete serial_port;
+        }
+        catch (std::exception &ex)
+        {
+            (void)ex; // call it to remove unused variable error
+            // This has been seen to happen before....
+        }
+        serial_port = 0;
+    }
+    serial_port = GetMaybeCreateSerialPort();
+    return serial_port;
+}
+
+SerialPort *SerialIO::GetMaybeCreateSerialPort()
+{
+    if (serial_port == 0)
+    {
+        try
+        {
+            Tracer::Trace("Opening %s serial port to communicate with navX-MXP/Micro.\n", (is_usb ? "USB " : "TTL UART "));
+            serial_port = new SerialPort(57600, serial_port_id);
+            serial_port->SetReadBufferSize(256);
+            serial_port->SetTimeout(units::second_t(1.0));
+            serial_port->EnableTermination('\n');
+            serial_port->Reset();
+        }
+        catch (std::exception &ex)
+        {
+            (void)ex; // call it to remove unused variable error
+            /* Error opening serial port. Perhaps it doesn't exist... */
+            serial_port = 0;
+        }
+    }
+    return serial_port;
+}
+
+void SerialIO::EnqueueIntegrationControlMessage(uint8_t action)
+{
+    next_integration_control_action = action;
+    signal_transmit_integration_control = true;
+}
+
+void SerialIO::DispatchStreamResponse(IMUProtocol::StreamResponse &response)
+{
+    board_state.cal_status = (uint8_t)(response.flags & NAV6_FLAG_MASK_CALIBRATION_STATE);
+    board_state.capability_flags = (int16_t)(response.flags & ~NAV6_FLAG_MASK_CALIBRATION_STATE);
+
+    /* Derive reasonable operational/self-test status from the available stream response data. */
+    if (board_state.cal_status == NAVX_CAL_STATUS_IMU_CAL_COMPLETE)
+    {
+        board_state.op_status = NAVX_OP_STATUS_NORMAL;
+    }
+    else
+    {
+        board_state.op_status = NAVX_OP_STATUS_IMU_AUTOCAL_IN_PROGRESS;
+    }
+    board_state.selftest_status = (NAVX_SELFTEST_STATUS_COMPLETE |
+                                   NAVX_SELFTEST_RESULT_GYRO_PASSED |
+                                   NAVX_SELFTEST_RESULT_ACCEL_PASSED |
+                                   NAVX_SELFTEST_RESULT_BARO_PASSED);
+
+    board_state.accel_fsr_g = response.accel_fsr_g;
+    board_state.gyro_fsr_dps = response.gyro_fsr_dps;
+    board_state.update_rate_hz = (uint8_t)response.update_rate_hz;
+    notify_sink->SetBoardState(board_state, true);
+    /* If AHRSPOS_TS is update type is requested, but board doesn't support it, */
+    /* retransmit the stream config, falling back to AHRSPos update mode, if    */
+    /* the board supports it, otherwise fall all the way back to AHRS Update mode. */
+    if (response.stream_type != this->update_type)
+    {
+        if (this->update_type == MSGID_AHRSPOS_TS_UPDATE)
+        {
+            if (board_capabilities->IsAHRSPosTimestampSupported())
+            {
+                this->update_type = MSGID_AHRSPOS_TS_UPDATE;
+            }
+            else if (board_capabilities->IsDisplacementSupported())
+            {
+                this->update_type = MSGID_AHRSPOS_UPDATE;
+            }
+            else
+            {
+                this->update_type = MSGID_AHRS_UPDATE;
+            }
+            signal_retransmit_stream_config = true;
+        }
+    }
+}
+
+int test_packet_length_debug = 65535;
+
+int SerialIO::DecodePacketHandler(char *received_data, int bytes_remaining)
+{
+    int packet_length;
+    long sensor_timestamp = 0; /* Serial protocols do not provide sensor timestamps. */
+
+    if ((packet_length = IMUProtocol::decodeYPRUpdate(received_data, bytes_remaining, ypr_update_data)))
+    {
+        notify_sink->SetYawPitchRoll(ypr_update_data, sensor_timestamp);
+    }
+    else if ((packet_length = AHRSProtocol::decodeAHRSPosTSUpdate(received_data, bytes_remaining, ahrspos_ts_update_data)))
+    {
+        if (ahrspos_ts_update_data.op_status != 0)
+        {
+            notify_sink->SetAHRSPosData(ahrspos_ts_update_data, ahrspos_ts_update_data.timestamp);
+        }
+    }
+    else if ((packet_length = AHRSProtocol::decodeAHRSPosUpdate(received_data, bytes_remaining, ahrspos_update_data)))
+    {
+        if (ahrspos_ts_update_data.op_status != 0)
+        {
+            notify_sink->SetAHRSPosData(ahrspos_update_data, sensor_timestamp);
+        }
+    }
+    else if ((packet_length = AHRSProtocol::decodeAHRSUpdate(received_data, bytes_remaining, ahrs_update_data)))
+    {
+        if (ahrspos_ts_update_data.op_status != 0)
+        {
+            notify_sink->SetAHRSData(ahrs_update_data, sensor_timestamp);
+        }
+    }
+    else if ((packet_length = IMUProtocol::decodeGyroUpdate(received_data, bytes_remaining, gyro_update_data)))
+    {
+        notify_sink->SetRawData(gyro_update_data, sensor_timestamp);
+    }
+    else if ((packet_length = AHRSProtocol::decodeBoardIdentityResponse(received_data, bytes_remaining, board_id)))
+    {
+        notify_sink->SetBoardID(board_id);
+    }
+    else
+    {
+        packet_length = 0;
+    }
+    return packet_length;
+}
+
+void SerialIO::Run()
+{
+    stop = false;
+    bool stream_response_received = false;
+    double last_stream_command_sent_timestamp = 0.0;
+    double last_data_received_timestamp = 0;
+    double last_second_start_time = 0;
+
+    int partial_binary_packet_count = 0;
+    int stream_response_receive_count = 0;
+    int timeout_count = 0;
+    int discarded_bytes_count = 0;
+    int port_reset_count = 0;
+    int updates_in_last_second = 0;
+    int integration_response_receive_count = 0;
+
+    try
+    {
+        serial_port->SetReadBufferSize(256);
+        serial_port->SetTimeout(units::second_t(1.0));
+        serial_port->EnableTermination('\n');
+        serial_port->Flush();
+        serial_port->Reset();
+    }
+    catch (std::exception &ex)
+    {
+        Tracer::Trace("SerialPort Run() Port Initialization Exception:  %s\n", ex.what());
+    }
+
+    char stream_command[256];
+    char integration_control_command[256];
+    IMUProtocol::StreamResponse response = {};
+    AHRSProtocol::IntegrationControl integration_control = {};
+    AHRSProtocol::IntegrationControl integration_control_response = {};
+
+    int cmd_packet_length = IMUProtocol::encodeStreamCommand(stream_command, update_type, update_rate_hz);
+    try
+    {
+        serial_port->Reset();
+        serial_port->Write(stream_command, cmd_packet_length);
+        cmd_packet_length = AHRSProtocol::encodeDataGetRequest(stream_command, AHRS_DATA_TYPE::BOARD_IDENTITY, AHRS_TUNING_VAR_ID::UNSPECIFIED);
+        serial_port->Write(stream_command, cmd_packet_length);
+        serial_port->Flush();
+        port_reset_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+        SmartDashboard::PutNumber("navX Port Resets", (double)port_reset_count);
+#endif
+        last_stream_command_sent_timestamp = unit_cast<double>(Timer::GetFPGATimestamp());
+    }
+    catch (std::exception &ex)
+    {
+        Tracer::Trace("SerialPort Run() Port Send Encode Stream Command Exception:  %s\n", ex.what());
+    }
+
+    int remainder_bytes = 0;
+    char received_data[256 * 3];
+    char additional_received_data[256];
+    char remainder_data[256];
+
+    while (!stop)
+    {
+        try
+        {
+
+            if (serial_port == NULL)
+            {
+                delayMillis(1000 / update_rate_hz);
+                if (debug)
+                    Tracer::Trace("Initiating reset of serial port, as serial_port reference is null.\n");
+                ResetSerialPort();
+                continue;
+            }
+
+            // Wait, with delays to conserve CPU resources, until
+            // bytes have arrived.
+
+            if (signal_transmit_integration_control)
+            {
+                integration_control.action = next_integration_control_action;
+                integration_control.parameter = 0xFFFFFFFF;
+                signal_transmit_integration_control = false;
+                next_integration_control_action = 0;
+                cmd_packet_length = AHRSProtocol::encodeIntegrationControlCmd(integration_control_command, integration_control);
+                try
+                {
+                    /* Ugly Hack.  This is a workaround for ARTF5478:           */
+                    /* (USB Serial Port Write hang if receive buffer not empty. */
+                    if (is_usb)
+                    {
+                        try
+                        {
+                            serial_port->Reset();
+                        }
+                        catch (std::exception &ex)
+                        {
+                            /* Sometimes an unclean status exception occurs during reset(). */
+                            ResetSerialPort();
+                            if (debug)
+                                Tracer::Trace("Exception during invocation of SerialPort::Reset:  %s\n", ex.what());
+                        }
+                    }
+                    int num_written = serial_port->Write(integration_control_command, cmd_packet_length);
+                    if (num_written != cmd_packet_length)
+                    {
+                        Tracer::Trace("Error writing integration control command.  Only %d of %d bytes were sent.\n", num_written, cmd_packet_length);
+                    }
+                    else
+                    {
+                        Tracer::Trace("Checksum:  %X %X\n", integration_control_command[9], integration_control_command[10]);
+                    }
+                    serial_port->Flush();
+                }
+                catch (std::exception &ex)
+                {
+                    Tracer::Trace("SerialPort Run() IntegrationControl Send Exception during Serial Port Write:  %s\n", ex.what());
+                }
+            }
+
+            if (!stop && (remainder_bytes == 0) && (serial_port->GetBytesReceived() < 1))
+            {
+                delayMillis(1000 / update_rate_hz);
+            }
+
+            int packets_received = 0;
+            int bytes_read = serial_port->Read(received_data, sizeof(received_data));
+            byte_count += bytes_read;
+
+            /* If a partial packet remains from last iteration, place that at  */
+            /* the start of the data buffer, and append any new data available */
+            /* at the serial port.                                             */
+
+            if (remainder_bytes > 0)
+            {
+                std::memcpy(received_data + bytes_read, remainder_data, remainder_bytes);
+                bytes_read += remainder_bytes;
+                remainder_bytes = 0;
+            }
+
+            if (bytes_read > 0)
+            {
+                last_data_received_timestamp = unit_cast<double>(Timer::GetFPGATimestamp());
+                int i = 0;
+                // Scan the buffer looking for valid packets
+                while (i < bytes_read)
+                {
+
+                    // Attempt to decode a packet
+
+                    int bytes_remaining = bytes_read - i;
+
+                    if (received_data[i] != PACKET_START_CHAR)
+                    {
+                        /* Skip over received bytes until a packet start is detected. */
+                        i++;
+                        discarded_bytes_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                        SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
+#endif
+                        continue;
+                    }
+                    else
+                    {
+                        if ((bytes_remaining > 2) &&
+                            (received_data[i + 1] == BINARY_PACKET_INDICATOR_CHAR))
+                        {
+                            /* Binary packet received; next byte is packet length-2 */
+                            uint8_t total_expected_binary_data_bytes = received_data[i + 2];
+                            total_expected_binary_data_bytes += 2;
+                            while (bytes_remaining < total_expected_binary_data_bytes)
+                            {
+
+                                /* This binary packet contains an embedded     */
+                                /* end-of-line character.  Continue to receive */
+                                /* more data until entire packet is received.  */
+                                int additional_received_data_length =
+                                    serial_port->Read(additional_received_data, sizeof(additional_received_data));
+                                byte_count += additional_received_data_length;
+
+                                /* Resize array to hold existing and new data */
+                                if (additional_received_data_length > 0)
+                                {
+                                    std::memcpy(received_data + bytes_remaining, additional_received_data, additional_received_data_length);
+                                    bytes_remaining += additional_received_data_length;
+                                }
+                                else
+                                {
+                                    /* Timeout waiting for remainder of binary packet */
+                                    i++;
+                                    bytes_remaining--;
+                                    partial_binary_packet_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                                    SmartDashboard::PutNumber("navX Partial Binary Packets", (double)partial_binary_packet_count);
+#endif
+                                    continue;
+                                }
+                            }
+                        }
+                    }
+
+                    int packet_length = DecodePacketHandler(received_data + i, bytes_remaining);
+                    if (packet_length > 0)
+                    {
+                        packets_received++;
+                        update_count++;
+                        if (!connect_reported)
+                        {
+                            notify_sink->ConnectDetected();
+                            connect_reported = true;
+                            disconnect_reported = false;
+                        }
+                        last_valid_packet_time = unit_cast<double>(Timer::GetFPGATimestamp());
+                        updates_in_last_second++;
+                        if ((last_valid_packet_time - last_second_start_time) > 1.0)
+                        {
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                            SmartDashboard::PutNumber("navX Updates Per Sec", (double)updates_in_last_second);
+#endif
+                            updates_in_last_second = 0;
+                            last_second_start_time = last_valid_packet_time;
+                        }
+                        i += packet_length;
+                    }
+                    else
+                    {
+                        packet_length = IMUProtocol::decodeStreamResponse(received_data + i, bytes_remaining, response);
+                        if (packet_length > 0)
+                        {
+                            packets_received++;
+                            if (!connect_reported)
+                            {
+                                notify_sink->ConnectDetected();
+                                connect_reported = true;
+                                disconnect_reported = false;
+                            }
+                            DispatchStreamResponse(response);
+                            stream_response_received = true;
+                            i += packet_length;
+                            stream_response_receive_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                            SmartDashboard::PutNumber("navX Stream Responses", (double)stream_response_receive_count);
+#endif
+                        }
+                        else
+                        {
+                            packet_length = AHRSProtocol::decodeIntegrationControlResponse(received_data + i, bytes_remaining,
+                                                                                           integration_control_response);
+                            if (packet_length > 0)
+                            {
+                                // Confirmation of integration control
+                                integration_response_receive_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                                SmartDashboard::PutNumber("navX Integration Control Response Count", integration_response_receive_count);
+#endif
+                                i += packet_length;
+                                if ((integration_control.action & NAVX_INTEGRATION_CTL_RESET_YAW) != 0)
+                                {
+                                    notify_sink->YawResetComplete();
+                                }
+                            }
+                            else
+                            {
+                                /* Even though a start-of-packet indicator was found, the  */
+                                /* current index is not the start of a packet if interest. */
+                                /* Scan to the beginning of the next packet,               */
+                                bool next_packet_start_found = false;
+                                int x;
+                                for (x = 0; x < bytes_remaining; x++)
+                                {
+                                    if (received_data[i + x] != PACKET_START_CHAR)
+                                    {
+                                        x++;
+                                    }
+                                    else
+                                    {
+                                        i += x;
+                                        bytes_remaining -= x;
+                                        if (x != 0)
+                                        {
+                                            next_packet_start_found = true;
+                                        }
+                                        break;
+                                    }
+                                }
+                                bool discard_remainder = false;
+                                if (!next_packet_start_found && x == bytes_remaining)
+                                {
+                                    /* Remaining bytes don't include a start-of-packet */
+                                    discard_remainder = true;
+                                }
+                                bool partial_packet = false;
+                                if (discard_remainder)
+                                {
+                                    /* Discard the remainder */
+                                    i = bytes_remaining;
+                                }
+                                else
+                                {
+                                    if (!next_packet_start_found)
+                                    {
+                                        /* This occurs when packets are received that are not decoded.   */
+                                        /* Bump over this packet and prepare for the next.               */
+                                        if ((bytes_remaining > 2) &&
+                                            (received_data[i + 1] == BINARY_PACKET_INDICATOR_CHAR))
+                                        {
+                                            /* Binary packet received; next byte is packet length-2 */
+                                            int pkt_len = received_data[i + 2];
+                                            pkt_len += 2;
+                                            if (bytes_remaining >= pkt_len)
+                                            {
+                                                bytes_remaining -= pkt_len;
+                                                i += pkt_len;
+                                                discarded_bytes_count += pkt_len;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                                                SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
+#endif
+                                            }
+                                            else
+                                            {
+                                                /* This is the initial portion of a partial binary packet. */
+                                                /* Keep this data and attempt to acquire the remainder.    */
+                                                partial_packet = true;
+                                            }
+                                        }
+                                        else
+                                        {
+                                            /* Ascii packet received. */
+                                            /* Scan up to and including next end-of-packet character       */
+                                            /* sequence, or the beginning of a new packet.                 */
+                                            for (x = 0; x < bytes_remaining; x++)
+                                            {
+                                                if (received_data[i + x] == '\r')
+                                                {
+                                                    i += x + 1;
+                                                    bytes_remaining -= (x + 1);
+                                                    discarded_bytes_count += x + 1;
+                                                    if ((bytes_remaining > 0) && received_data[i] == '\n')
+                                                    {
+                                                        bytes_remaining--;
+                                                        i++;
+                                                        discarded_bytes_count++;
+                                                    }
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                                                    SmartDashboard::PutNumber("navX Discarded Bytes", (double)discarded_bytes_count);
+#endif
+                                                    break;
+                                                }
+                                                /* If a new start-of-packet is found, discard */
+                                                /* the ascii packet bytes that precede it.    */
+                                                if (received_data[i + x] == '!')
+                                                {
+                                                    if (x > 0)
+                                                    {
+                                                        i += x;
+                                                        bytes_remaining -= x;
+                                                        discarded_bytes_count += x;
+                                                        break;
+                                                    }
+                                                    else
+                                                    {
+                                                        /* start of packet found, but no termination     */
+                                                        /* Time to get some more data, unless the bytes  */
+                                                        /* remaining are larger than a valid packet size */
+                                                        if (bytes_remaining < IMU_PROTOCOL_MAX_MESSAGE_LENGTH)
+                                                        {
+                                                            /* Get more data */
+                                                            partial_packet = true;
+                                                        }
+                                                        else
+                                                        {
+                                                            i++;
+                                                            bytes_remaining--;
+                                                        }
+                                                        break;
+                                                    }
+                                                }
+                                            }
+                                            if (x == bytes_remaining)
+                                            {
+                                                /* Partial ascii packet - keep the remainder */
+                                                partial_packet = true;
+                                            }
+                                        }
+                                    }
+                                }
+                                if (partial_packet)
+                                {
+                                    if (bytes_remaining > (int)sizeof(remainder_data))
+                                    {
+                                        std::memcpy(remainder_data, received_data + i - sizeof(remainder_data), sizeof(remainder_data));
+                                        remainder_bytes = sizeof(remainder_data);
+                                    }
+                                    else
+                                    {
+                                        std::memcpy(remainder_data, received_data + i, bytes_remaining);
+                                        remainder_bytes = bytes_remaining;
+                                    }
+                                    i = bytes_read;
+                                }
+                            }
+                        }
+                    }
+                }
+
+                if ((packets_received == 0) && (bytes_read == 256))
+                {
+                    // Workaround for issue found in SerialPort implementation:
+                    // No packets received and 256 bytes received; this
+                    // condition occurs in the SerialPort.  In this case,
+                    // reset the serial port.
+                    serial_port->Flush();
+                    serial_port->Reset();
+                    port_reset_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+                    SmartDashboard::PutNumber("navX Port Resets", (double)port_reset_count);
+#endif
+                }
+
+                bool retransmit_stream_config = false;
+                if (signal_retransmit_stream_config)
+                {
+                    retransmit_stream_config = true;
+                    signal_retransmit_stream_config = false;
+                }
+
+                // If a stream configuration response has not been received within three seconds
+                // of operation, (re)send a stream configuration request
+
+                if (retransmit_stream_config ||
+                    (!stream_response_received && ((unit_cast<double>(Timer::GetFPGATimestamp()) - last_stream_command_sent_timestamp) > 3.0)))
+                {
+                    cmd_packet_length = IMUProtocol::encodeStreamCommand(stream_command, update_type, update_rate_hz);
+                    try
+                    {
+                        Tracer::Trace("Retransmitting stream configuration command to navX-MXP/Micro.\n");
+                        /* Ugly Hack.  This is a workaround for ARTF5478:           */
+                        /* (USB Serial Port Write hang if receive buffer not empty. */
+                        if (is_usb)
+                        {
+                            try
+                            {
+                                serial_port->Reset();
+                            }
+                            catch (std::exception &ex)
+                            {
+                                /* Sometimes an unclean status exception occurs during reset(). */
+                                ResetSerialPort();
+                                if (debug)
+                                    Tracer::Trace("Exception during invocation of SerialPort::Reset:  %s\n", ex.what());
+                            }
+                        }
+                        serial_port->Write(stream_command, cmd_packet_length);
+                        cmd_packet_length = AHRSProtocol::encodeDataGetRequest(stream_command, AHRS_DATA_TYPE::BOARD_IDENTITY, AHRS_TUNING_VAR_ID::UNSPECIFIED);
+                        serial_port->Write(stream_command, cmd_packet_length);
+                        serial_port->Flush();
+                        last_stream_command_sent_timestamp = unit_cast<double>(Timer::GetFPGATimestamp());
+                    }
+                    catch (std::exception &ex2)
+                    {
+                        Tracer::Trace("SerialPort Run() Re-transmit Encode Stream Command Exception:  %s\n", ex2.what());
+                    }
+                }
+                else
+                {
+                    // If no bytes remain in the buffer, and not awaiting a response, sleep a bit
+                    if (stream_response_received && (serial_port->GetBytesReceived() == 0))
+                    {
+                        delayMillis(1000 / update_rate_hz);
+                    }
+                }
+
+                /* If receiving data, but no valid packets have been received in the last second */
+                /* the navX MXP may have been reset, but no exception has been detected.         */
+                /* In this case , trigger transmission of a new stream_command, to ensure the    */
+                /* streaming packet type is configured correctly.                                */
+
+                if ((unit_cast<double>(Timer::GetFPGATimestamp()) - last_valid_packet_time) > 1.0)
+                {
+                    last_stream_command_sent_timestamp = 0.0;
+                    stream_response_received = false;
+                }
+            }
+            else
+            {
+                /* No data received this time around */
+                if (unit_cast<double>(Timer::GetFPGATimestamp()) - last_data_received_timestamp > 1.0)
+                {
+                    if (debug)
+                        Tracer::Trace("Initiating Serial Port Reset since no data was received in the last second.\n");
+                    ResetSerialPort();
+                }
+            }
+        }
+        catch (std::exception &ex)
+        {
+            // This exception typically indicates a Timeout, but can also be a buffer overrun error.
+            stream_response_received = false;
+            timeout_count++;
+#ifdef SERIALIO_DASHBOARD_DEBUG
+            SmartDashboard::PutNumber("navX Serial Port Timeout / Buffer Overrun", (double)timeout_count);
+            SmartDashboard::PutString("navX Last Exception", ex.what());
+#endif
+            if (debug)
+                Tracer::Trace("Initiating Serial Port Reset due to exception during Run() loop.\n");
+            ResetSerialPort();
+        }
+    }
+}
+
+bool SerialIO::IsConnected()
+{
+    double time_since_last_update = unit_cast<double>(Timer::GetFPGATimestamp()) - this->last_valid_packet_time;
+    return time_since_last_update <= IO_TIMEOUT_SECONDS;
+}
+
+double SerialIO::GetByteCount()
+{
+    return byte_count;
+}
+
+double SerialIO::GetUpdateCount()
+{
+    return update_count;
+}
+
+void SerialIO::SetUpdateRateHz(uint8_t update_rate)
+{
+    update_rate_hz = update_rate;
+}
+
+void SerialIO::ZeroYaw()
+{
+    EnqueueIntegrationControlMessage(NAVX_INTEGRATION_CTL_RESET_YAW);
+}
+
+void SerialIO::ZeroDisplacement()
+{
+    EnqueueIntegrationControlMessage(NAVX_INTEGRATION_CTL_RESET_DISP_X |
+                                     NAVX_INTEGRATION_CTL_RESET_DISP_Y |
+                                     NAVX_INTEGRATION_CTL_RESET_DISP_Z);
+}
+
+void SerialIO::Stop()
+{
+    stop = true;
+}
+
+void SerialIO::EnableLogging(bool enable)
+{
+    debug = enable;
+}
diff --git a/src/main/native/cpp/SerialIO.h b/src/main/native/cpp/SerialIO.h
new file mode 100644
index 0000000..34fef3c
--- /dev/null
+++ b/src/main/native/cpp/SerialIO.h
@@ -0,0 +1,75 @@
+/*
+ * SerialIO.h
+ *
+ *  Created on: Jul 31, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_SERIALIO_H_
+#define SRC_SERIALIO_H_
+
+#include "IIOProvider.h"
+#include "AHRSProtocol.h"
+#include "IMUProtocol.h"
+#include "IIOCompleteNotification.h"
+#include "IBoardCapabilities.h"
+
+#include <frc/SerialPort.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include <stdint.h>
+
+using namespace frc;
+
+class SerialIO : public IIOProvider
+{
+    SerialPort::Port serial_port_id;
+    SerialPort *serial_port;
+    uint8_t next_integration_control_action;
+    bool signal_transmit_integration_control;
+    bool signal_retransmit_stream_config;
+    bool stop;
+    uint8_t update_type; // IMUProtocol.MSGID_XXX
+    uint8_t update_rate_hz;
+    int byte_count;
+    int update_count;
+    IMUProtocol::YPRUpdate ypr_update_data;
+    IMUProtocol::GyroUpdate gyro_update_data;
+    AHRSProtocol::AHRSUpdate ahrs_update_data;
+    AHRSProtocol::AHRSPosUpdate ahrspos_update_data;
+    AHRSProtocol::AHRSPosTSUpdate ahrspos_ts_update_data;
+    AHRSProtocol::BoardID board_id;
+    IIOCompleteNotification *notify_sink;
+    IIOCompleteNotification::BoardState board_state;
+    IBoardCapabilities *board_capabilities;
+    double last_valid_packet_time;
+    bool is_usb;
+    bool connect_reported;
+    bool disconnect_reported;
+    bool debug;
+
+public:
+    SerialIO(SerialPort::Port port_id,
+             uint8_t update_rate_hz,
+             bool processed_data,
+             IIOCompleteNotification *notify_sink,
+             IBoardCapabilities *board_capabilities);
+    bool IsConnected();
+    double GetByteCount();
+    double GetUpdateCount();
+    void SetUpdateRateHz(uint8_t update_rate);
+    void ZeroYaw();
+    void ZeroDisplacement();
+    void Run();
+    void Stop();
+    void EnableLogging(bool enable);
+
+private:
+    SerialPort *ResetSerialPort();
+    SerialPort *GetMaybeCreateSerialPort();
+    void EnqueueIntegrationControlMessage(uint8_t action);
+    void DispatchStreamResponse(IMUProtocol::StreamResponse &response);
+    int DecodePacketHandler(char *received_data, int bytes_remaining);
+};
+
+#endif /* SRC_SERIALIO_H_ */
diff --git a/src/main/native/cpp/SimIO.cpp b/src/main/native/cpp/SimIO.cpp
new file mode 100644
index 0000000..70ec704
--- /dev/null
+++ b/src/main/native/cpp/SimIO.cpp
@@ -0,0 +1,306 @@
+/*
+ * SimIO.cpp
+ *
+ *  Created on: Jul 29, 2015
+ *      Author: Scott
+ */
+
+#include "SimIO.h"
+#include "IMURegisters.h"
+#include "delay.h"
+
+#include <units/base.h>
+
+using namespace frc;
+using units::unit_cast;
+
+SimIO::SimIO(uint8_t update_rate_hz,
+             IIOCompleteNotification *notify_sink,
+             hal::SimDevice *sim_device)
+{
+    this->update_rate_hz = update_rate_hz;
+    this->notify_sink = notify_sink;
+    this->stop = false;
+    this->start_seconds = 0.0;
+    this->is_connected = false;
+    this->sim_device = sim_device;
+
+    raw_data_update = {};
+    ahrs_update = {};
+    board_state = {};
+    board_id = {};
+
+    if (sim_device)
+    {
+
+        auto const direction = hal::SimDevice::Direction::kInput;
+        // Booleans
+
+        this->simConnected = sim_device->CreateBoolean("Connected", direction, true);
+
+        // Doubles
+        this->simYaw = sim_device->CreateDouble("Yaw", direction, 0.0f);
+        this->simPitch = sim_device->CreateDouble("Pitch", direction, 0.0f);
+        this->simRoll = sim_device->CreateDouble("Roll", direction, 0.0f);
+        this->simCompassHeading = sim_device->CreateDouble("CompassHeading", direction, 0.0f);
+        this->simFusedHeading = sim_device->CreateDouble("FusedHeading", direction, 0.0f);
+
+        this->simLinearWorldAccelX = sim_device->CreateDouble("LinearWorldAccelX", direction, 0.0f);
+        this->simLinearWorldAccelY = sim_device->CreateDouble("LinearWorldAccelY", direction, 0.0f);
+        this->simLinearWorldAccelZ = sim_device->CreateDouble("LinearWorldAccelZ", direction, 0.0f);
+
+        board_id.fw_ver_major = 3;
+        board_id.fw_ver_minor = 1;
+        board_id.fw_revision = 400;
+        board_id.type = 33; // navx-MXP type id
+
+        board_state.selftest_status =
+            NAVX_SELFTEST_STATUS_COMPLETE |
+            NAVX_SELFTEST_RESULT_GYRO_PASSED |
+            NAVX_SELFTEST_RESULT_ACCEL_PASSED |
+            NAVX_SELFTEST_RESULT_MAG_PASSED; // BARO Passed is NOT set
+        board_state.sensor_status =
+            // NAVX_SENSOR_STATUS_MOVING |              // NOTE:  Updated by Sim Variable
+            NAVX_SENSOR_STATUS_YAW_STABLE |
+            // NAVX_SENSOR_STATUS_MAG_DISTURBANCE |     // NOTE:  Always false
+            // NAVX_SENSOR_STATUS_ALTITUDE_VALID |      // NOTE:  Always false
+            // NAVX_SENSOR_STATUS_SEALEVEL_PRESS_SET |  // NOTE:  Always false
+            NAVX_SENSOR_STATUS_FUSED_HEADING_VALID;
+        board_state.op_status =
+            NAVX_OP_STATUS_NORMAL;
+        board_state.cal_status =
+            NAV6_CALIBRATION_STATE_COMPLETE;
+        board_state.capability_flags =
+            // Note:  Configure capabilities to NOT include VEL_AND_DISP,
+            // this forces the internal InertialDataIntegrator to be used.
+            // NAVX_CAPABILITY_FLAG_VEL_AND_DISP |
+            NAVX_CAPABILITY_FLAG_OMNIMOUNT |
+            NAVX_CAPABILITY_FLAG_AHRSPOS_TS |
+            // Note:  Configure capabilities to NOT include HW-based YAW RESET;
+            // This causes software-based yaw reset to be used
+            // NAVX_CAPABILITY_FLAG_YAW_RESET |
+            OMNIMOUNT_DEFAULT;
+        board_state.accel_fsr_g = 2;
+        board_state.gyro_fsr_dps = 2000;
+        board_state.update_rate_hz = this->update_rate_hz;
+
+        /////////////////////////
+        // AHRSUpdate initialization
+        /////////////////////////
+
+        ahrs_update.mpu_temp = 35.0f;
+
+        // Set Quaternion values to default (identity)
+        ahrs_update.quat_w = 0.0f;
+        ahrs_update.quat_x = 0.0f;
+        ahrs_update.quat_y = 0.0f;
+        ahrs_update.quat_z = 1.0f;
+
+        ahrs_update.vel_x = 0.0f;
+        ahrs_update.vel_y = 0.0f;
+        ahrs_update.vel_z = 0.0f;
+
+        ahrs_update.disp_x = 0.0f;
+        ahrs_update.disp_y = 0.0f;
+        ahrs_update.disp_z = 0.0f;
+
+        ahrs_update.altitude = 50.0f;               // Average surface altitude, planet earth
+        ahrs_update.barometric_pressure = 1013.25f; // Nominal pressure at earth's surface
+
+        // Synchronize AHRS Update and Board State values
+
+        ahrs_update.cal_status = board_state.cal_status;
+        ahrs_update.op_status = board_state.op_status;
+        ahrs_update.selftest_status = board_state.selftest_status;
+        ahrs_update.sensor_status = board_state.sensor_status;
+
+        // The following values are updated by Sim Variables later; set to reasonable defaults now
+
+        ahrs_update.linear_accel_x = 0.0f;
+        ahrs_update.linear_accel_y = 0.0f;
+        ahrs_update.linear_accel_z = 0.0f;
+        ahrs_update.yaw = 0.0f;
+        ahrs_update.pitch = 0.0f;
+        ahrs_update.roll = 0.0f;
+        ahrs_update.compass_heading = 0.0f;
+        ahrs_update.fused_heading = 0.0f;
+
+        // RawUpdate initialization
+
+        raw_data_update.mag_x = 40;
+        raw_data_update.mag_y = 40;
+        raw_data_update.mag_z = 40;
+
+        // Set simulated raw accel/gyro values to values representing noise
+        raw_data_update.accel_x = 50;
+        raw_data_update.accel_y = 50;
+        raw_data_update.accel_z = 50;
+
+        raw_data_update.gyro_x = 50;
+        raw_data_update.gyro_y = 50;
+        raw_data_update.gyro_z = 50;
+
+        // State history initialization
+        last_yaw = 0;
+        last_linear_world_accel_x = 0;
+        last_linear_world_accel_y = 0;
+
+        printf("navX-Sensor SimDevice created.\n");
+    }
+}
+
+SimIO::~SimIO()
+{
+}
+
+bool SimIO::IsConnected()
+{
+    return is_connected;
+}
+
+double SimIO::GetByteCount()
+{
+#define SIM_BYTES_PER_SECOND 1000
+    double num_secs_running = unit_cast<double>(Timer::GetFPGATimestamp()) - start_seconds;
+    return num_secs_running * SIM_BYTES_PER_SECOND;
+}
+
+double SimIO::GetUpdateCount()
+{
+    double num_secs_running = unit_cast<double>(Timer::GetFPGATimestamp()) - start_seconds;
+    return num_secs_running / update_rate_hz;
+}
+
+void SimIO::SetUpdateRateHz(uint8_t update_rate)
+{
+    this->update_rate_hz = update_rate;
+}
+
+void SimIO::ZeroYaw()
+{
+    notify_sink->YawResetComplete();
+}
+
+void SimIO::ZeroDisplacement()
+{
+}
+
+void SimIO::Run()
+{
+    /* IO Loop */
+    // Simulate startup delay
+    delayMillis(50);
+
+    // Default to connected state
+    is_connected = true;
+    notify_sink->ConnectDetected();
+
+    long sensor_timestamp = 2000; // NOTE:  Simulate a 2-second navX-sensor firmware startup delay
+
+    // Update all static values
+    notify_sink->SetBoardID(board_id);
+    notify_sink->SetRawData(raw_data_update, sensor_timestamp);
+
+    // Update AHRS data (portions of which are static; others are updated from sim variables)
+    notify_sink->SetAHRSPosData(ahrs_update, sensor_timestamp);
+
+    start_seconds = unit_cast<double>(Timer::GetFPGATimestamp());
+
+    while (!stop)
+    {
+        delayMillis(20);
+        sensor_timestamp += 20;
+        UpdatePeriodicFromSimVariables(sensor_timestamp);
+    }
+}
+
+float SimIO::CalculateNormal(float in1, float in2)
+{
+    return fabs(sqrt(fabs(in1 * in1) + fabs(in2 * in2)));
+}
+
+void SimIO::UpdatePeriodicFromSimVariables(long sensor_timestamp)
+{
+
+    if (!sim_device)
+        return;
+
+    bool curr_is_connected = simConnected.Get();
+
+    // Update connection state
+    if (curr_is_connected != is_connected)
+    {
+        is_connected = curr_is_connected;
+        if (curr_is_connected)
+        {
+            notify_sink->ConnectDetected();
+        }
+        else
+        {
+            notify_sink->DisconnectDetected();
+        }
+    }
+
+    if (curr_is_connected)
+    {
+        ahrs_update.yaw = (float)simYaw.Get();
+        ahrs_update.pitch = (float)simPitch.Get();
+        ahrs_update.roll = (float)simRoll.Get();
+        ahrs_update.compass_heading = (float)simCompassHeading.Get();
+        ahrs_update.fused_heading = (float)simFusedHeading.Get();
+        ahrs_update.linear_accel_x = (float)simLinearWorldAccelX.Get();
+        ahrs_update.linear_accel_y = (float)simLinearWorldAccelY.Get();
+        ahrs_update.linear_accel_z = (float)simLinearWorldAccelZ.Get();
+
+        // Detect motion
+        float last_linear_accel_norm = CalculateNormal(last_linear_world_accel_x, last_linear_world_accel_y);
+        float curr_linear_accel_norm = CalculateNormal(ahrs_update.linear_accel_x, ahrs_update.linear_accel_y);
+        bool curr_is_moving = (fabs(curr_linear_accel_norm - last_linear_accel_norm) >= 0.02f);
+        if (curr_is_moving)
+        {
+            board_state.sensor_status |= NAVX_SENSOR_STATUS_MOVING;
+        }
+        else
+        {
+            board_state.sensor_status &= ~(NAVX_SENSOR_STATUS_MOVING);
+        }
+
+        // Detect rotation
+        bool curr_is_rotating = fabs(last_yaw - ahrs_update.yaw) >= 0.05f;
+        if (!curr_is_rotating)
+        {
+            board_state.sensor_status |= NAVX_SENSOR_STATUS_YAW_STABLE;
+        }
+        else
+        {
+            board_state.sensor_status &= ~(NAVX_SENSOR_STATUS_YAW_STABLE);
+        }
+
+        ahrs_update.sensor_status = board_state.sensor_status;
+
+        // Trigger simulated update to notification sink
+        notify_sink->SetAHRSPosData(ahrs_update, sensor_timestamp);
+
+        // Update cached state variables
+        last_yaw = ahrs_update.yaw;
+        last_linear_world_accel_x = ahrs_update.linear_accel_x;
+        last_linear_world_accel_y = ahrs_update.linear_accel_y;
+    }
+}
+
+double SimIO::GetRate()
+{
+    if (!sim_device)
+        return 0;
+
+    return simRate.Get();
+}
+
+void SimIO::Stop()
+{
+    stop = true;
+}
+
+void SimIO::EnableLogging(bool enable)
+{
+}
diff --git a/src/main/native/cpp/SimIO.h b/src/main/native/cpp/SimIO.h
new file mode 100644
index 0000000..d35b031
--- /dev/null
+++ b/src/main/native/cpp/SimIO.h
@@ -0,0 +1,74 @@
+/*
+ * SimIO.h
+ *
+ *  Created on: Jan 28, 2020
+ *      Author: Scott
+ */
+
+#ifndef SRC_SIMIO_H_
+#define SRC_SIMIO_H_
+
+#include "IIOProvider.h"
+#include "IMUProtocol.h"
+#include "AHRSProtocol.h"
+#include "IBoardCapabilities.h"
+#include "IIOCompleteNotification.h"
+
+#include <frc/Timer.h>
+#include <hal/SimDevice.h>
+
+#include <stdint.h>
+
+class SimIO : public IIOProvider
+{
+private:
+    bool stop;
+    bool is_connected;
+    double start_seconds;
+    IIOCompleteNotification *notify_sink;
+    uint8_t update_rate_hz;
+
+    hal::SimDevice *sim_device;
+    hal::SimBoolean simConnected;
+    hal::SimDouble simRate;
+    hal::SimDouble simYaw;
+    hal::SimDouble simPitch;
+    hal::SimDouble simRoll;
+    hal::SimDouble simCompassHeading;
+    hal::SimDouble simFusedHeading;
+    hal::SimDouble simLinearWorldAccelX;
+    hal::SimDouble simLinearWorldAccelY;
+    hal::SimDouble simLinearWorldAccelZ;
+
+    float last_yaw;
+    float last_linear_world_accel_x;
+    float last_linear_world_accel_y;
+
+    AHRSProtocol::BoardID board_id;
+    IIOCompleteNotification::BoardState board_state;
+    AHRSProtocol::AHRSPosUpdate ahrs_update;
+    IMUProtocol::GyroUpdate raw_data_update;
+
+public:
+    SimIO(uint8_t update_rate_hz,
+          IIOCompleteNotification *notify_sink,
+          hal::SimDevice *sim_device);
+    bool IsConnected();
+    double GetByteCount();
+    double GetUpdateCount();
+    void SetUpdateRateHz(uint8_t update_rate);
+    void ZeroYaw();
+    void ZeroDisplacement();
+    void Run();
+    void Stop();
+    void EnableLogging(bool enable);
+    virtual ~SimIO();
+
+    double GetRate();
+
+private:
+    void UpdatePeriodicFromSimVariables(long sensor_timestamp);
+    float CalculateNormal(float in1, float in2);
+};
+
+#endif /* SRC_SIMIO_H_ */
diff --git a/src/main/native/cpp/Tracer.cpp b/src/main/native/cpp/Tracer.cpp
new file mode 100644
index 0000000..8a54e8a
--- /dev/null
+++ b/src/main/native/cpp/Tracer.cpp
@@ -0,0 +1,45 @@
+#include "Tracer.h"
+
+#include <frc/Timer.h>
+#include <units/base.h>
+#include <units/time.h>
+
+#include <cstdarg>
+#include <cstdio>
+#include <string.h>
+
+using units::unit_cast;
+using namespace frc;
+
+char Tracer::lastMsg[TRACE_BUFFER_LEN] = {0};
+double Tracer::lastMsgTimestamp = 0.0;
+
+const double minDelta = 0.5;
+
+void Tracer::Trace(const char *pMsg, ...)
+{
+    char buffer[TRACE_BUFFER_LEN];
+    std::va_list arg;
+
+    double msgTimestamp = unit_cast<double>(Timer::GetFPGATimestamp());
+    double delta = msgTimestamp - lastMsgTimestamp;
+
+    va_start(arg, pMsg);
+    std::vsnprintf(buffer, TRACE_BUFFER_LEN, pMsg, arg);
+    if ((strcmp(buffer, Tracer::lastMsg) != 0) ||
+        (delta >= minDelta))
+    {
+        printf("%s", buffer);
+        size_t len = strlen(buffer);
+        // Truncate string if longer than last message buffer
+        if (len > (TRACE_BUFFER_LEN - 1))
+        {
+            len = TRACE_BUFFER_LEN - 1;
+        }
+        memcpy(Tracer::lastMsg, buffer, len);
+        Tracer::lastMsg[len] = 0;
+        lastMsgTimestamp = msgTimestamp;
+    }
+
+    va_end(arg);
+}
diff --git a/src/main/native/cpp/Tracer.h b/src/main/native/cpp/Tracer.h
new file mode 100644
index 0000000..52da290
--- /dev/null
+++ b/src/main/native/cpp/Tracer.h
@@ -0,0 +1,16 @@
+#ifndef SRC_TRACER_H_
+#define SRC_TRACER_H_
+
+#define TRACE_BUFFER_LEN 4096
+
+class Tracer
+{
+private:
+    static char lastMsg[TRACE_BUFFER_LEN];
+    static double lastMsgTimestamp;
+
+public:
+    static void Trace(const char *pMsg, ...);
+};
+
+#endif /* SRC_TRACER_H_ */
\ No newline at end of file
diff --git a/src/main/native/cpp/delay.h b/src/main/native/cpp/delay.h
new file mode 100644
index 0000000..a84332d
--- /dev/null
+++ b/src/main/native/cpp/delay.h
@@ -0,0 +1,8 @@
+#include <iostream>
+#include <chrono>
+#include <thread>
+
+inline void delayMillis(int ms)
+{
+	std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+}
diff --git a/src/main/native/cpp/source.cpp b/src/main/native/cpp/source.cpp
deleted file mode 100644
index 0999576..0000000
--- a/src/main/native/cpp/source.cpp
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "header.h"
-void func(){}
\ No newline at end of file
diff --git a/src/main/native/include/AHRS.h b/src/main/native/include/AHRS.h
new file mode 100644
index 0000000..cd2033c
--- /dev/null
+++ b/src/main/native/include/AHRS.h
@@ -0,0 +1,248 @@
+/*
+ * AHRS.h
+ *
+ *  Created on: Jul 30, 2015
+ *      Author: Scott
+ */
+
+#ifndef SRC_AHRS_H_
+#define SRC_AHRS_H_
+
+#include "ITimestampedDataSubscriber.h"
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+#include <frc/I2C.h>
+#include <frc/SPI.h>
+#include <frc/SerialPort.h>
+#include <frc/Timer.h>
+#include <frc/interfaces/Gyro.h>
+#include <networktables/NetworkTableEntry.h>
+#include <hal/SimDevice.h>
+
+#include <thread>
+
+class IIOProvider;
+class ContinuousAngleTracker;
+class InertialDataIntegrator;
+class OffsetTracker;
+class AHRSInternal;
+
+/// Class providing an "Attitude and Heading Reference System" (AHRS)
+/// interface to a navX-sensor
+class AHRS : public frc::Gyro,
+             public wpi::Sendable,
+             public wpi::SendableHelper<AHRS>
+{
+public:
+    /// Enumeration of all board axes
+    enum BoardAxis
+    {
+        /// Board X (left/right) Axis
+        kBoardAxisX = 0,
+        /// Board Y (forward/reverse) Axis
+        kBoardAxisY = 1,
+        /// Board Z (up/down) Axis
+        kBoardAxisZ = 2,
+    };
+
+    /// Structure describing the current board orientation w/respect to the IMU axes of measurement.
+    struct BoardYawAxis
+    {
+        /// Identifies one of the board axes.
+        BoardAxis board_axis;
+        /// true if axis is pointing up (with respect to gravity); false if pointing down.
+        bool up;
+    };
+
+    /// navX-Sensor support Serial Data types
+    enum SerialDataType
+    {
+        /**
+         * (default):  6 and 9-axis processed data
+         */
+        kProcessedData = 0,
+        /**
+         * unprocessed data from each individual sensor
+         */
+        kRawData = 1
+    };
+
+private:
+    friend class AHRSInternal;
+    AHRSInternal *ahrs_internal;
+
+    volatile float yaw;
+    volatile float pitch;
+    volatile float roll;
+    volatile float compass_heading;
+    volatile float world_linear_accel_x;
+    volatile float world_linear_accel_y;
+    volatile float world_linear_accel_z;
+    volatile float mpu_temp_c;
+    volatile float fused_heading;
+    volatile float altitude;
+    volatile float baro_pressure;
+    volatile bool is_moving;
+    volatile bool is_rotating;
+    volatile float baro_sensor_temp_c;
+    volatile bool altitude_valid;
+    volatile bool is_magnetometer_calibrated;
+    volatile bool magnetic_disturbance;
+    volatile float quaternionW;
+    volatile float quaternionX;
+    volatile float quaternionY;
+    volatile float quaternionZ;
+
+    /* Integrated Data */
+    float velocity[3];
+    float displacement[3];
+
+    /* Raw Data */
+    volatile int16_t raw_gyro_x;
+    volatile int16_t raw_gyro_y;
+    volatile int16_t raw_gyro_z;
+    volatile int16_t raw_accel_x;
+    volatile int16_t raw_accel_y;
+    volatile int16_t raw_accel_z;
+    volatile int16_t cal_mag_x;
+    volatile int16_t cal_mag_y;
+    volatile int16_t cal_mag_z;
+
+    /* Configuration/Status */
+    volatile uint8_t update_rate_hz;
+    volatile int16_t accel_fsr_g;
+    volatile int16_t gyro_fsr_dps;
+    volatile int16_t capability_flags;
+    volatile uint8_t op_status;
+    volatile int16_t sensor_status;
+    volatile uint8_t cal_status;
+    volatile uint8_t selftest_status;
+
+    /* Board ID */
+    volatile uint8_t board_type;
+    volatile uint8_t hw_rev;
+    volatile uint8_t fw_ver_major;
+    volatile uint8_t fw_ver_minor;
+
+    long last_sensor_timestamp;
+    double last_update_time;
+
+    InertialDataIntegrator *integrator;
+    ContinuousAngleTracker *yaw_angle_tracker;
+    OffsetTracker *yaw_offset_tracker;
+    IIOProvider *io;
+
+    std::thread *task;
+
+    // Simulation
+    hal::SimDevice m_simDevice;
+
+#define MAX_NUM_CALLBACKS 3
+    ITimestampedDataSubscriber *callbacks[MAX_NUM_CALLBACKS];
+    void *callback_contexts[MAX_NUM_CALLBACKS];
+
+    bool enable_boardlevel_yawreset;
+    double last_yawreset_request_timestamp;
+    double last_yawreset_while_calibrating_request_timestamp;
+    uint32_t successive_suppressed_yawreset_request_count;
+    bool disconnect_startupcalibration_recovery_pending;
+    bool logging_enabled;
+
+public:
+    AHRS(frc::SPI::Port spi_port_id);
+    AHRS(frc::I2C::Port i2c_port_id);
+    AHRS(frc::SerialPort::Port serial_port_id);
+
+    AHRS(frc::SPI::Port spi_port_id, uint8_t update_rate_hz);
+    AHRS(frc::SPI::Port spi_port_id, uint32_t spi_bitrate, uint8_t update_rate_hz);
+
+    AHRS(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz);
+
+    AHRS(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
+
+    float GetPitch();
+    float GetRoll();
+    float GetYaw();
+    float GetCompassHeading();
+    void ZeroYaw();
+    bool IsCalibrating();
+    bool IsConnected();
+    double GetByteCount();
+    double GetUpdateCount();
+    long GetLastSensorTimestamp();
+    float GetWorldLinearAccelX();
+    float GetWorldLinearAccelY();
+    float GetWorldLinearAccelZ();
+    bool IsMoving();
+    bool IsRotating();
+    float GetBarometricPressure();
+    float GetAltitude();
+    bool IsAltitudeValid();
+    float GetFusedHeading();
+    bool IsMagneticDisturbance();
+    bool IsMagnetometerCalibrated();
+    float GetQuaternionW();
+    float GetQuaternionX();
+    float GetQuaternionY();
+    float GetQuaternionZ();
+    void ResetDisplacement();
+    void UpdateDisplacement(float accel_x_g, float accel_y_g,
+                            int update_rate_hz, bool is_moving);
+    float GetVelocityX();
+    float GetVelocityY();
+    float GetVelocityZ();
+    float GetDisplacementX();
+    float GetDisplacementY();
+    float GetDisplacementZ();
+    double GetAngle() const override;
+    double GetRate() const override;
+    void SetAngleAdjustment(double angle);
+    double GetAngleAdjustment();
+    void Reset() override;
+    float GetRawGyroX();
+    float GetRawGyroY();
+    float GetRawGyroZ();
+    float GetRawAccelX();
+    float GetRawAccelY();
+    float GetRawAccelZ();
+    float GetRawMagX();
+    float GetRawMagY();
+    float GetRawMagZ();
+    float GetPressure();
+    float GetTempC();
+    AHRS::BoardYawAxis GetBoardYawAxis();
+    std::string GetFirmwareVersion();
+
+    bool RegisterCallback(ITimestampedDataSubscriber *callback, void *callback_context);
+    bool DeregisterCallback(ITimestampedDataSubscriber *callback);
+
+    int GetActualUpdateRate();
+    int GetRequestedUpdateRate();
+
+    void EnableLogging(bool enable);
+    void EnableBoardlevelYawReset(bool enable);
+    bool IsBoardlevelYawResetEnabled();
+
+    int16_t GetGyroFullScaleRangeDPS();
+    int16_t GetAccelFullScaleRangeG();
+
+    // Gyro interface implementation
+    void Calibrate() override;
+
+private:
+    void SPIInit(frc::SPI::Port spi_port_id, uint32_t bitrate, uint8_t update_rate_hz);
+    void I2CInit(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz);
+    void SerialInit(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, uint8_t update_rate_hz);
+    void commonInit(uint8_t update_rate_hz);
+    static int ThreadFunc(IIOProvider *io_provider);
+
+    /* SendableBase implementation */
+    void InitSendable(wpi::SendableBuilder &builder) override;
+
+    uint8_t GetActualUpdateRateInternal(uint8_t update_rate);
+
+    nt::NetworkTableEntry m_valueEntry;
+};
+
+#endif /* SRC_AHRS_H_ */
diff --git a/src/main/native/include/AHRSProtocol.h b/src/main/native/include/AHRSProtocol.h
new file mode 100644
index 0000000..893072f
--- /dev/null
+++ b/src/main/native/include/AHRSProtocol.h
@@ -0,0 +1,1224 @@
+/* ============================================
+navX MXP source code is placed under the MIT license
+Copyright (c) 2015 Kauai Labs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+ */
+
+#ifndef _AHRS_PROTOCOL_H_
+#define _AHRS_PROTOCOL_H_
+
+/*****************************************************************************/
+/* This protocol, introduced first with the navX MXP, expands upon the IMU   */
+/* protocol by adding the following new functionality:                       */
+/*         																	 */
+/* AHRS Update:  Includes Fused Heading and Altitude Info                    */
+/* Magnetometer Calibration:  Enables configuration of coefficients from PC  */
+/* Board Identity:  Enables retrieval of Board Identification Info           */
+/* Fusion Tuning:  Enables configuration of key thresholds/coefficients used */
+/*                 in data fusion algorithms from a remote client            */
+/*                                                                           */
+/* In addition, the navX enable stream command has been extended with a new  */
+/* Stream type, in order to enable AHRS Updates.                             */
+/*****************************************************************************/
+
+#include "IMUProtocol.h"
+#include "IMURegisters.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#define BINARY_PACKET_INDICATOR_CHAR '#'
+
+/* AHRS Protocol encodes certain data in binary format, unlike the IMU  */
+/* protocol, which encodes all data in ASCII characters.  Thus, the     */
+/* packet start and message termination sequences may occur within the  */
+/* message content itself.  To support the binary format, the binary    */
+/* message has this format:                                             */
+/*                                                                      */
+/* [start][binary indicator][len][msgid]<MESSAGE>[checksum][terminator] */
+/*                                                                      */
+/* (The binary indicator and len are not present in the ASCII protocol) */
+/*                                                                      */
+/* The [len] does not include the length of the start and binary        */
+/* indicator characters, but does include all other message items,      */
+/* including the checksum and terminator sequence.                      */
+
+typedef enum
+{
+    UNSPECIFIED = 0,
+    MOTION_THRESHOLD = 1,             /* In G */
+    YAW_STABLE_THRESHOLD = 2,         /* In Degrees */
+    MAG_DISTURBANCE_THRESHOLD = 3,    /* Ratio */
+    SEA_LEVEL_PRESSURE = 4,           /* Millibars */
+    KALMAN_STATIC_MOTION_COEFF = 5,   /* [DEPRECATED] Unitless, from 0.5 to 10.0 */
+    KALMAN_DYNAMIC_MOTION_COEFF = 6,  /* [DEPRECATED] Unitless, from 0.5 to 10.0 */
+    YAW_GYRO_SCALE_FACTOR_RATIO = 7,  /* Unitless, from 0.9 to 1.1 */
+    MAX_GYRO_MEASUREMENT_ERR_DPS = 8, /* Sensor Fusion Gyro/(Acc+Mag) Beta Seed */
+    GYRO_FULL_SCALE_RANGE_DPS = 9,    /* In Degrees/Second */
+    ACCEL_FULL_SCALE_RANGE_G = 10,    /* In G */
+    MIN_TUNING_VAR_ID = MOTION_THRESHOLD,
+    MAX_TUNING_VAR_ID = ACCEL_FULL_SCALE_RANGE_G,
+} AHRS_TUNING_VAR_ID;
+
+typedef enum
+{
+    TUNING_VARIABLE = 0,
+    MAG_CALIBRATION = 1,
+    BOARD_IDENTITY = 2
+} AHRS_DATA_TYPE;
+
+typedef enum
+{
+    DATA_GET = 0,
+    DATA_SET = 1,
+    DATA_SET_TO_DEFAULT = 2,
+} AHRS_DATA_ACTION;
+
+#define DATA_GETSET_SUCCESS 0
+#define DATA_GETSET_ERROR 1
+
+// AHRS Update Packet - e.g., !a[yaw][pitch][roll][heading][altitude][fusedheading][accelx/y/z][angular rot x/y/z][opstatus][fusionstatus][cr][lf]
+
+#define MSGID_AHRS_UPDATE 'a'
+#define AHRS_UPDATE_YAW_VALUE_INDEX 4                 /* Degrees.  Signed Hundredths */
+#define AHRS_UPDATE_ROLL_VALUE_INDEX 6                /* Degrees.  Signed Hundredths */
+#define AHRS_UPDATE_PITCH_VALUE_INDEX 8               /* Degrees.  Signed Hundredeths */
+#define AHRS_UPDATE_HEADING_VALUE_INDEX 10            /* Degrees.  Unsigned Hundredths */
+#define AHRS_UPDATE_ALTITUDE_VALUE_INDEX 12           /* Meters.   Signed 16:16 */
+#define AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX 16      /* Degrees.  Unsigned Hundredths */
+#define AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX 18     /* Inst. G.  Signed Thousandths */
+#define AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX 20     /* Inst. G.  Signed Thousandths */
+#define AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX 22     /* Inst. G.  Signed Thousandths */
+#define AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX 24          /* Int16 (Device Units) */
+#define AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX 26          /* Int16 (Device Units) */
+#define AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX 28          /* Int16 (Device Units) */
+#define AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX 30 /* Ratio.  Unsigned Hundredths */
+#define AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX 32     /* Coefficient. Signed q16:16 */
+#define AHRS_UPDATE_MPU_TEMP_VAUE_INDEX 36            /* Centigrade.  Signed Hundredths */
+#define AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX 38          /* INT16 (Device Units) */
+#define AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX 40          /* INT16 (Device Units) */
+#define AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX 42          /* INT16 (Device Units) */
+#define AHRS_UPDATE_QUAT_W_VALUE_INDEX 44             /* INT16 */
+#define AHRS_UPDATE_QUAT_X_VALUE_INDEX 46             /* INT16 */
+#define AHRS_UPDATE_QUAT_Y_VALUE_INDEX 48             /* INT16 */
+#define AHRS_UPDATE_QUAT_Z_VALUE_INDEX 50             /* INT16 */
+#define AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX 52      /* millibar.  Signed 16:16 */
+#define AHRS_UPDATE_BARO_TEMP_VAUE_INDEX 56           /* Centigrade.  Signed  Hundredths */
+#define AHRS_UPDATE_OPSTATUS_VALUE_INDEX 58           /* NAVX_OP_STATUS_XXX */
+#define AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX 59      /* NAVX_SENSOR_STATUS_XXX */
+#define AHRS_UPDATE_CAL_STATUS_VALUE_INDEX 60         /* NAVX_CAL_STATUS_XXX */
+#define AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX 61    /* NAVX_SELFTEST_STATUS_XXX */
+#define AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX 62
+#define AHRS_UPDATE_MESSAGE_TERMINATOR_INDEX 64
+#define AHRS_UPDATE_MESSAGE_LENGTH 66
+
+// AHRSAndPositioning Update Packet (similar to AHRS, but removes magnetometer and adds velocity/displacement) */
+
+#define MSGID_AHRSPOS_UPDATE 'p'
+#define AHRSPOS_UPDATE_YAW_VALUE_INDEX 4              /* Degrees.  Signed Hundredths */
+#define AHRSPOS_UPDATE_ROLL_VALUE_INDEX 6             /* Degrees.  Signed Hundredths */
+#define AHRSPOS_UPDATE_PITCH_VALUE_INDEX 8            /* Degrees.  Signed Hundredeths */
+#define AHRSPOS_UPDATE_HEADING_VALUE_INDEX 10         /* Degrees.  Unsigned Hundredths */
+#define AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX 12        /* Meters.   Signed 16:16 */
+#define AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX 16   /* Degrees.  Unsigned Hundredths */
+#define AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX 18  /* Inst. G.  Signed Thousandths */
+#define AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX 20  /* Inst. G.  Signed Thousandths */
+#define AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX 22  /* Inst. G.  Signed Thousandths */
+#define AHRSPOS_UPDATE_VEL_X_VALUE_INDEX 24           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX 28           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX 32           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_UPDATE_DISP_X_VALUE_INDEX 36          /* Signed 16:16, in meters */
+#define AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX 40          /* Signed 16:16, in meters */
+#define AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX 44          /* Signed 16:16, in meters */
+#define AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX 48          /* INT16 */
+#define AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX 50          /* INT16 */
+#define AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX 52          /* INT16 */
+#define AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX 54          /* INT16 */
+#define AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX 56         /* Centigrade.  Signed Hundredths */
+#define AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX 58        /* NAVX_OP_STATUS_XXX */
+#define AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX 59   /* NAVX_SENSOR_STATUS_XXX */
+#define AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX 60      /* NAVX_CAL_STATUS_XXX */
+#define AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX 61 /* NAVX_SELFTEST_STATUS_XXX */
+#define AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX 62
+#define AHRSPOS_UPDATE_MESSAGE_TERMINATOR_INDEX 64
+#define AHRSPOS_UPDATE_MESSAGE_LENGTH 66
+
+// AHRSAndPositioningWithTimestamp Update Packet (similar to AHRSPos, but adds sample timestamp)
+
+#define MSGID_AHRSPOS_TS_UPDATE 't'
+#define AHRSPOS_TS_UPDATE_YAW_VALUE_INDEX 4              /* Signed 16:16.  Signed Hundredths */
+#define AHRSPOS_TS_UPDATE_ROLL_VALUE_INDEX 8             /* Signed 16:16.  Signed Hundredths */
+#define AHRSPOS_TS_UPDATE_PITCH_VALUE_INDEX 12           /* Signed 16:16.  Signed Hundredeths */
+#define AHRSPOS_TS_UPDATE_HEADING_VALUE_INDEX 16         /* Signed 16:16.  Unsigned Hundredths */
+#define AHRSPOS_TS_UPDATE_ALTITUDE_VALUE_INDEX 20        /* Meters.   Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_FUSED_HEADING_VALUE_INDEX 24   /* Degrees.  Unsigned Hundredths */
+#define AHRSPOS_TS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX 28  /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX 32  /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX 36  /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_VEL_X_VALUE_INDEX 40           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_UPDATE_VEL_Y_VALUE_INDEX 44           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_UPDATE_VEL_Z_VALUE_INDEX 48           /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_UPDATE_DISP_X_VALUE_INDEX 52          /* Signed 16:16, in meters */
+#define AHRSPOS_TS_UPDATE_DISP_Y_VALUE_INDEX 56          /* Signed 16:16, in meters */
+#define AHRSPOS_TS_UPDATE_DISP_Z_VALUE_INDEX 60          /* Signed 16:16, in meters */
+#define AHRSPOS_TS_UPDATE_QUAT_W_VALUE_INDEX 64          /* Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_QUAT_X_VALUE_INDEX 68          /* Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_QUAT_Y_VALUE_INDEX 72          /* Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_QUAT_Z_VALUE_INDEX 76          /* Signed 16:16 */
+#define AHRSPOS_TS_UPDATE_MPU_TEMP_VAUE_INDEX 80         /* Centigrade.  Signed Hundredths */
+#define AHRSPOS_TS_UPDATE_OPSTATUS_VALUE_INDEX 82        /* NAVX_OP_STATUS_XXX */
+#define AHRSPOS_TS_UPDATE_SENSOR_STATUS_VALUE_INDEX 83   /* NAVX_SENSOR_STATUS_XXX */
+#define AHRSPOS_TS_UPDATE_CAL_STATUS_VALUE_INDEX 84      /* NAVX_CAL_STATUS_XXX */
+#define AHRSPOS_TS_UPDATE_SELFTEST_STATUS_VALUE_INDEX 85 /* NAVX_SELFTEST_STATUS_XXX */
+#define AHRSPOS_TS_UPDATE_TIMESTAMP_INDEX 86             /* UINT32 Timestamp, in milliseconds */
+#define AHRSPOS_TS_UPDATE_MESSAGE_CHECKSUM_INDEX 90
+#define AHRSPOS_TS_UPDATE_MESSAGE_TERMINATOR_INDEX 92
+#define AHRSPOS_TS_UPDATE_MESSAGE_LENGTH 94
+
+// AHRSAndPositioningWithTimestampAndRaw Update Packet (similar to AHRSPosTS, but adds raw data)
+
+#define MSGID_AHRSPOS_TS_RAW_UPDATE 'u'
+#define AHRSPOS_TS_RAW_UPDATE_YAW_VALUE_INDEX 4                /* Signed 16:16.  Signed Hundredths */
+#define AHRSPOS_TS_RAW_UPDATE_ROLL_VALUE_INDEX 8               /* Signed 16:16.  Signed Hundredths */
+#define AHRSPOS_TS_RAW_UPDATE_PITCH_VALUE_INDEX 12             /* Signed 16:16.  Signed Hundredeths */
+#define AHRSPOS_TS_RAW_UPDATE_HEADING_VALUE_INDEX 16           /* Signed 16:16.  Unsigned Hundredths */
+#define AHRSPOS_TS_RAW_UPDATE_ALTITUDE_VALUE_INDEX 20          /* Meters.   Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_FUSED_HEADING_VALUE_INDEX 24     /* Degrees.  Unsigned Hundredths */
+#define AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX 28    /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX 32    /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX 36    /* Inst. G.  Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_VEL_X_VALUE_INDEX 40             /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_RAW_UPDATE_VEL_Y_VALUE_INDEX 44             /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_RAW_UPDATE_VEL_Z_VALUE_INDEX 48             /* Signed 16:16, in meters/sec */
+#define AHRSPOS_TS_RAW_UPDATE_DISP_X_VALUE_INDEX 52            /* Signed 16:16, in meters */
+#define AHRSPOS_TS_RAW_UPDATE_DISP_Y_VALUE_INDEX 56            /* Signed 16:16, in meters */
+#define AHRSPOS_TS_RAW_UPDATE_DISP_Z_VALUE_INDEX 60            /* Signed 16:16, in meters */
+#define AHRSPOS_TS_RAW_UPDATE_QUAT_W_VALUE_INDEX 64            /* Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_QUAT_X_VALUE_INDEX 68            /* Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_QUAT_Y_VALUE_INDEX 72            /* Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_QUAT_Z_VALUE_INDEX 76            /* Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_MPU_TEMP_VAUE_INDEX 80           /* Centigrade.  Signed Hundredths */
+#define AHRSPOS_TS_RAW_UPDATE_OPSTATUS_VALUE_INDEX 82          /* NAVX_OP_STATUS_XXX */
+#define AHRSPOS_TS_RAW_UPDATE_SENSOR_STATUS_VALUE_INDEX 83     /* NAVX_SENSOR_STATUS_XXX */
+#define AHRSPOS_TS_RAW_UPDATE_CAL_STATUS_VALUE_INDEX 84        /* NAVX_CAL_STATUS_XXX */
+#define AHRSPOS_TS_RAW_UPDATE_SELFTEST_STATUS_VALUE_INDEX 85   /* NAVX_SELFTEST_STATUS_XXX */
+#define AHRSPOS_TS_RAW_UPDATE_TIMESTAMP_INDEX 86               /* UINT32 Timestamp, in milliseconds */
+#define AHRSPOS_TS_RAW_UPDATE_GYRO_X_VALUE_INDEX 90            /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_GYRO_Y_VALUE_INDEX 92            /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_GYRO_Z_VALUE_INDEX 94            /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_ACC_X_VALUE_INDEX 96             /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_ACC_Y_VALUE_INDEX 98             /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_ACC_Z_VALUE_INDEX 100            /* INT16, in device units (scaled by FSR) */
+#define AHRSPOS_TS_RAW_UPDATE_LAST_SAMPLE_DELTA_SEC_INDEX 104  /* Signed 16:16 */
+#define AHRSPOS_TS_RAW_UPDATE_GYRO_BIAS_UPDATED_BOOL_INDEX 108 /* INT16, 0 if false, otherwise true */
+#define AHRSPOS_TS_RAW_UPDATE_MESSAGE_CHECKSUM_INDEX 110
+#define AHRSPOS_TS_RAW_UPDATE_MESSAGE_TERMINATOR_INDEX 112
+#define AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH 114
+
+// Data Get Request:  Tuning Variable, Mag Cal, Board Identity (Response message depends upon request type)
+#define MSGID_DATA_REQUEST 'D'
+#define DATA_REQUEST_DATATYPE_VALUE_INDEX 4
+#define DATA_REQUEST_VARIABLEID_VALUE_INDEX 5
+#define DATA_REQUEST_CHECKSUM_INDEX 6
+#define DATA_REQUEST_TERMINATOR_INDEX 8
+#define DATA_REQUEST_MESSAGE_LENGTH 10
+
+/* Data Set Response Packet (in response to MagCal SET and Tuning SET commands. */
+#define MSGID_DATA_SET_RESPONSE 'v'
+#define DATA_SET_RESPONSE_DATATYPE_VALUE_INDEX 4
+#define DATA_SET_RESPONSE_VARID_VALUE_INDEX 5
+#define DATA_SET_RESPONSE_STATUS_VALUE_INDEX 6
+#define DATA_SET_RESPONSE_MESSAGE_CHECKSUM_INDEX 7
+#define DATA_SET_RESPONSE_MESSAGE_TERMINATOR_INDEX 9
+#define DATA_SET_RESPONSE_MESSAGE_LENGTH 11
+
+/* Integration Control Command Packet */
+#define MSGID_INTEGRATION_CONTROL_CMD 'I'
+#define INTEGRATION_CONTROL_CMD_ACTION_INDEX 4
+#define INTEGRATION_CONTROL_CMD_PARAMETER_INDEX 5
+#define INTEGRATION_CONTROL_CMD_MESSAGE_CHECKSUM_INDEX 9
+#define INTEGRATION_CONTROL_CMD_MESSAGE_TERMINATOR_INDEX 11
+#define INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH 13
+
+/* Integration Control Response Packet */
+#define MSGID_INTEGRATION_CONTROL_RESP 'j'
+#define INTEGRATION_CONTROL_RESP_ACTION_INDEX 4
+#define INTEGRATION_CONTROL_RESP_PARAMETER_INDEX 5
+#define INTEGRATION_CONTROL_RESP_MESSAGE_CHECKSUM_INDEX 9
+#define INTEGRATION_CONTROL_RESP_MESSAGE_TERMINATOR_INDEX 11
+#define INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH 13
+
+/* Magnetometer Calibration Packet
+ ** This message may be used to SET (store) a new calibration into the navX board, or may be used
+ ** to retrieve the current calibration data from the navX board. */
+#define MSGID_MAG_CAL_CMD 'M'
+#define MAG_CAL_DATA_ACTION_VALUE_INDEX 4
+#define MAG_X_BIAS_VALUE_INDEX 5 /* signed short */
+#define MAG_Y_BIAS_VALUE_INDEX 7
+#define MAG_Z_BIAS_VALUE_INDEX 9
+#define MAG_XFORM_1_1_VALUE_INDEX 11 /* signed 16:16 */
+#define MAG_XFORM_1_2_VALUE_INDEX 15
+#define MAG_XFORM_1_3_VALUE_INDEX 19
+#define MAG_XFORM_2_1_VALUE_INDEX 23
+#define MAG_XFORM_2_2_VALUE_INDEX 27
+#define MAG_XFORM_2_3_VALUE_INDEX 31
+#define MAG_XFORM_3_1_VALUE_INDEX 35
+#define MAG_XFORM_3_2_VALUE_INDEX 39
+#define MAG_XFORM_3_3_VALUE_INDEX 43
+#define MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX 47
+#define MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX 51
+#define MAG_CAL_CMD_MESSAGE_TERMINATOR_INDEX 53
+#define MAG_CAL_CMD_MESSAGE_LENGTH 55
+
+/* Tuning Variable Packet
+ ** This message may be used to SET (modify) a tuning variable into the navX board,
+ ** or to retrieve a current tuning variable from the navX board. */
+#define MSGID_FUSION_TUNING_CMD 'T'
+#define FUSION_TUNING_DATA_ACTION_VALUE_INDEX 4
+#define FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX 5
+#define FUSION_TUNING_CMD_VAR_VALUE_INDEX 6
+#define FUSION_TUNING_CMD_MESSAGE_CHECKSUM_INDEX 10
+#define FUSION_TUNING_CMD_MESSAGE_TERMINATOR_INDEX 12
+#define FUSION_TUNING_CMD_MESSAGE_LENGTH 14
+
+// Board Identity Response Packet
+// Sent in response to a Data Get (Board ID) message
+#define MSGID_BOARD_IDENTITY_RESPONSE 'i'
+#define BOARD_IDENTITY_BOARDTYPE_VALUE_INDEX 4
+#define BOARD_IDENTITY_HWREV_VALUE_INDEX 5
+#define BOARD_IDENTITY_FW_VER_MAJOR 6
+#define BOARD_IDENTITY_FW_VER_MINOR 7
+#define BOARD_IDENTITY_FW_VER_REVISION_VALUE_INDEX 8
+#define BOARD_IDENTITY_UNIQUE_ID_0 10
+#define BOARD_IDENTITY_UNIQUE_ID_1 11
+#define BOARD_IDENTITY_UNIQUE_ID_2 12
+#define BOARD_IDENTITY_UNIQUE_ID_3 13
+#define BOARD_IDENTITY_UNIQUE_ID_4 14
+#define BOARD_IDENTITY_UNIQUE_ID_5 15
+#define BOARD_IDENTITY_UNIQUE_ID_6 16
+#define BOARD_IDENTITY_UNIQUE_ID_7 17
+#define BOARD_IDENTITY_UNIQUE_ID_8 18
+#define BOARD_IDENTITY_UNIQUE_ID_9 19
+#define BOARD_IDENTITY_UNIQUE_ID_10 20
+#define BOARD_IDENTITY_UNIQUE_ID_11 21
+#define BOARD_IDENTITY_RESPONSE_CHECKSUM_INDEX 22
+#define BOARD_IDENTITY_RESPONSE_TERMINATOR_INDEX 24
+#define BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH 26
+
+// NavX2-specific messages
+
+#define MSGID_CAL_STATUS 'c'
+#define CAL_STATUS_TYPE_INDEX 4      // 'A' : Accel, 'M' : Magnetometer
+#define CAL_STATUS_STATE_INDEX 5     // 'D' : Disabled, 'E' : Enabled
+#define CAL_STATUS_QUALITY_INDEX 6   // NONE, POOR, OK, GOOD
+#define CAL_STATUS_PARAMETER_INDEX 7 // 0-3 (StatUpdate, StatResponse, CmdAck, CmdNAK)
+#define CAL_STATUS_CHECKSUM_INDEX 11
+#define CAL_STATUS_TERMINATOR_INDEX 13
+#define CAL_STATUS_MESSAGE_LENGTH 15
+
+/* Calibration Control Command Packet */
+#define MSGID_CAL_CONTROL 'C'
+#define CAL_CONTROL_TYPE_INDEX 4      // 'A' : Accel, 'M' : Magmetometer
+#define CAL_CONTROL_CMD_INDEX 5       // 'S' : Start, 'P' : Stop, 'R' : StatusRequest
+#define CAL_CONTROL_PARAMETER_INDEX 6 // Currently Unused
+#define CAL_CONTROL_CMD_MESSAGE_CHECKSUM_INDEX 10
+#define CAL_CONTROL_CMD_MESSAGE_TERMINATOR_INDEX 12
+#define CAL_CONTROL_CMD_MESSAGE_LENGTH 14
+
+#define AHRS_PROTOCOL_MAX_MESSAGE_LENGTH AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH
+
+enum CAL_CMD
+{
+    CAL_CMD_START = 'S',
+    CAL_CMD_STOP = 'P',
+    CAL_CMD_STATUS_REQUEST = 'R'
+};
+enum CAL_TYPE
+{
+    CAL_TYPE_ACCEL = 'A',
+    CAL_TYPE_MAGNETOMETER = 'M'
+};
+enum CAL_STATE
+{
+    CAL_STATE_NONE = 0,
+    CAL_STATE_INPROGRESS = 1,
+    CAL_STATE_DONE = 2
+};
+enum CAL_QUALITY
+{
+    CAL_QUAL_NONE = 0,
+    CAL_QUAL_POOR = 1,
+    CAL_QUAL_OK = 2,
+    CAL_QUAL_GOOD = 3
+};
+enum CAL_PARAMETER
+{
+    CAL_PARAM_NOTIFY = 0,
+    CAL_PARAM_STATUS_RESPONSE = 1,
+    CAL_PARAM_ACK = 2,
+    CAL_PARAM_NACK = 3
+};
+
+class AHRSProtocol : public IMUProtocol
+{
+public:
+    struct AHRSUpdateBase
+    {
+        float yaw;
+        float pitch;
+        float roll;
+        float compass_heading;
+        float altitude;
+        float fused_heading;
+        float linear_accel_x;
+        float linear_accel_y;
+        float linear_accel_z;
+        float mpu_temp;
+        float quat_w;
+        float quat_x;
+        float quat_y;
+        float quat_z;
+        float barometric_pressure;
+        float baro_temp;
+        uint8_t op_status;
+        uint8_t sensor_status;
+        uint8_t cal_status;
+        uint8_t selftest_status;
+    };
+
+    struct AHRSUpdate : public AHRSUpdateBase
+    {
+        short cal_mag_x;
+        short cal_mag_y;
+        short cal_mag_z;
+        float mag_field_norm_ratio;
+        float mag_field_norm_scalar;
+        short raw_mag_x;
+        short raw_mag_y;
+        short raw_mag_z;
+    };
+
+    struct AHRSPosUpdate : public AHRSUpdateBase
+    {
+        float vel_x;
+        float vel_y;
+        float vel_z;
+        float disp_x;
+        float disp_y;
+        float disp_z;
+    };
+
+    struct AHRSPosTSUpdate : public AHRSPosUpdate
+    {
+        uint32_t timestamp;
+    };
+
+    struct AHRSPosTSRawUpdate : public AHRSPosTSUpdate
+    {
+        int16_t raw_gyro_x;     // Device units, scaled by FSR
+        int16_t raw_gyro_y;     // Device units, scaled by FSR
+        int16_t raw_gyro_z;     // Device units, scaled by FSR
+        int16_t accel_x;        // Device units, scaled by FSR
+        int16_t accel_y;        // Device units, scaled by FSR
+        int16_t accel_z;        // Device units, scaled by FSR
+        float delta_t_sec;      // last sample period, as a portion of a second
+        bool gyro_bias_updated; // true if gyro bias was updated (e.g., when still) during last sample
+    };
+
+    struct BoardID
+    {
+        uint8_t type;
+        uint8_t hw_rev;
+        uint8_t fw_ver_major;
+        uint8_t fw_ver_minor;
+        int16_t fw_revision;
+        uint8_t unique_id[12];
+    };
+
+    struct IntegrationControl
+    {
+        uint8_t action;
+        int parameter;
+    };
+
+    struct CalibrationControl
+    {
+        uint8_t type;    // CAL_TYPE_xxx
+        uint8_t command; // CAL_CMD_xxx
+        int parameter;   // Unused
+    };
+
+    struct CalibrationStatus
+    {
+        uint8_t type;        // CAL_TYPE_xxx
+        uint8_t state;       // CAL_STATE_xxx
+        uint8_t cal_quality; // CAL_QUALITY_xxx
+        int parameter;       // CAL_PARAM_xxx
+    };
+
+public:
+    static int encodeIntegrationControlCmd(char *protocol_buffer, struct IntegrationControl &cmd)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_INTEGRATION_CONTROL_CMD;
+        // Data
+        protocol_buffer[INTEGRATION_CONTROL_CMD_ACTION_INDEX] = cmd.action;
+        IMURegisters::encodeProtocolInt32(cmd.parameter, &protocol_buffer[INTEGRATION_CONTROL_CMD_PARAMETER_INDEX]);
+        // Footer
+        encodeTermination(protocol_buffer, INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH, INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH - 4);
+        return INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH;
+    }
+
+    static int decodeIntegrationControlCmd(char *buffer, int length, uint8_t &action, int32_t &parameter)
+    {
+        if (length < INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_INTEGRATION_CONTROL_CMD))
+        {
+            if (!verifyChecksum(buffer, INTEGRATION_CONTROL_CMD_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            // Data
+            action = (uint8_t)buffer[INTEGRATION_CONTROL_CMD_ACTION_INDEX];
+            parameter = IMURegisters::decodeProtocolInt32(&buffer[INTEGRATION_CONTROL_CMD_PARAMETER_INDEX]);
+            return INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeIntegrationControlResponse(char *protocol_buffer, uint8_t action, int32_t parameter)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_INTEGRATION_CONTROL_RESP;
+        // Data
+        protocol_buffer[INTEGRATION_CONTROL_RESP_ACTION_INDEX] = action;
+        IMURegisters::encodeProtocolInt32(parameter, &protocol_buffer[INTEGRATION_CONTROL_RESP_PARAMETER_INDEX]);
+        // Footer
+        encodeTermination(protocol_buffer, INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH, INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH - 4);
+        return INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH;
+    }
+
+    static int decodeIntegrationControlResponse(char *buffer, int length, struct IntegrationControl &rsp)
+    {
+        if (length < INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_INTEGRATION_CONTROL_RESP))
+        {
+            if (!verifyChecksum(buffer, INTEGRATION_CONTROL_RESP_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            // Data
+            rsp.action = (uint8_t)buffer[INTEGRATION_CONTROL_RESP_ACTION_INDEX];
+            rsp.parameter = IMURegisters::decodeProtocolInt32(&buffer[INTEGRATION_CONTROL_RESP_PARAMETER_INDEX]);
+            return INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeCalibrationControlCmd(char *protocol_buffer, struct CalibrationControl &cmd)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = CAL_CONTROL_CMD_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_CAL_CONTROL;
+        // Data
+        protocol_buffer[CAL_CONTROL_TYPE_INDEX] = cmd.type;
+        protocol_buffer[CAL_CONTROL_CMD_INDEX] = cmd.command;
+        IMURegisters::encodeProtocolInt32(cmd.parameter, &protocol_buffer[CAL_CONTROL_PARAMETER_INDEX]);
+        // Footer
+        encodeTermination(protocol_buffer, CAL_CONTROL_CMD_MESSAGE_LENGTH, CAL_CONTROL_CMD_MESSAGE_LENGTH - 4);
+        return CAL_CONTROL_CMD_MESSAGE_LENGTH;
+    }
+
+    static int decodeCalibrationControlCmd(char *buffer, int length, struct CalibrationControl &cmd)
+    {
+        if (length < CAL_CONTROL_CMD_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == CAL_CONTROL_CMD_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_CAL_CONTROL))
+        {
+            if (!verifyChecksum(buffer, CAL_CONTROL_CMD_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            // Data
+            cmd.type = (uint8_t)buffer[CAL_CONTROL_TYPE_INDEX];
+            cmd.command = (uint8_t)buffer[CAL_CONTROL_CMD_INDEX];
+            cmd.parameter = IMURegisters::decodeProtocolInt32(&buffer[CAL_CONTROL_PARAMETER_INDEX]);
+            return CAL_CONTROL_CMD_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeCalibrationStatus(char *protocol_buffer, struct CalibrationStatus &status)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = CAL_STATUS_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_CAL_STATUS;
+        // Data
+        protocol_buffer[CAL_STATUS_STATE_INDEX] = status.state;
+        protocol_buffer[CAL_STATUS_TYPE_INDEX] = status.type;
+        protocol_buffer[CAL_STATUS_QUALITY_INDEX] = status.cal_quality;
+        IMURegisters::encodeProtocolInt32(status.parameter, &protocol_buffer[CAL_STATUS_PARAMETER_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, CAL_STATUS_MESSAGE_LENGTH, CAL_STATUS_MESSAGE_LENGTH - 4);
+        return CAL_STATUS_MESSAGE_LENGTH;
+    }
+
+    static int encodeTuningVariableCmd(char *protocol_buffer, AHRS_DATA_ACTION getset, AHRS_TUNING_VAR_ID id, float val)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = FUSION_TUNING_CMD_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_FUSION_TUNING_CMD;
+        // Data
+        protocol_buffer[FUSION_TUNING_DATA_ACTION_VALUE_INDEX] = getset;
+        protocol_buffer[FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX] = id;
+        IMURegisters::encodeProtocol1616Float(val, &protocol_buffer[FUSION_TUNING_CMD_VAR_VALUE_INDEX]);
+        // Footer
+        encodeTermination(protocol_buffer, FUSION_TUNING_CMD_MESSAGE_LENGTH, FUSION_TUNING_CMD_MESSAGE_LENGTH - 4);
+        return FUSION_TUNING_CMD_MESSAGE_LENGTH;
+    }
+
+    static int decodeTuningVariableCmd(char *buffer, int length, AHRS_DATA_ACTION &getset, AHRS_TUNING_VAR_ID &id, float &val)
+    {
+        if (length < FUSION_TUNING_CMD_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == FUSION_TUNING_CMD_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_FUSION_TUNING_CMD))
+        {
+            if (!verifyChecksum(buffer, FUSION_TUNING_CMD_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            // Data
+            getset = (AHRS_DATA_ACTION)buffer[FUSION_TUNING_DATA_ACTION_VALUE_INDEX];
+            id = (AHRS_TUNING_VAR_ID)buffer[FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX];
+            val = IMURegisters::decodeProtocol1616Float(&buffer[FUSION_TUNING_CMD_VAR_VALUE_INDEX]);
+            return FUSION_TUNING_CMD_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeAHRSUpdate(char *protocol_buffer,
+                                float yaw, float pitch, float roll,
+                                float compass_heading, float altitude, float fused_heading,
+                                float linear_accel_x, float linear_accel_y, float linear_accel_z,
+                                float mpu_temp_c,
+                                int16_t raw_mag_x, int16_t raw_mag_y, int16_t raw_mag_z,
+                                int16_t cal_mag_x, int16_t cal_mag_y, int16_t cal_mag_z,
+                                float mag_norm_ratio, float mag_norm_scalar,
+                                int16_t quat_w, int16_t quat_x, int16_t quat_y, int16_t quat_z,
+                                float baro_pressure, float baro_temp_c,
+                                uint8_t op_status, uint8_t sensor_status,
+                                uint8_t cal_status, uint8_t selftest_status)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = AHRS_UPDATE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_AHRS_UPDATE;
+        // data
+        IMURegisters::encodeProtocolSignedHundredthsFloat(yaw, &protocol_buffer[AHRS_UPDATE_YAW_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(pitch, &protocol_buffer[AHRS_UPDATE_PITCH_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(roll, &protocol_buffer[AHRS_UPDATE_ROLL_VALUE_INDEX]);
+        IMURegisters::encodeProtocolUnsignedHundredthsFloat(compass_heading, &protocol_buffer[AHRS_UPDATE_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(altitude, &protocol_buffer[AHRS_UPDATE_ALTITUDE_VALUE_INDEX]);
+        IMURegisters::encodeProtocolUnsignedHundredthsFloat(fused_heading, &protocol_buffer[AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_x, &protocol_buffer[AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_y, &protocol_buffer[AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_z, &protocol_buffer[AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(cal_mag_x, &protocol_buffer[AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(cal_mag_y, &protocol_buffer[AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(cal_mag_z, &protocol_buffer[AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolUnsignedHundredthsFloat(mag_norm_ratio, &protocol_buffer[AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(mag_norm_scalar, &protocol_buffer[AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(mpu_temp_c, &protocol_buffer[AHRS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_mag_x, &protocol_buffer[AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_mag_y, &protocol_buffer[AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_mag_z, &protocol_buffer[AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_w, &protocol_buffer[AHRS_UPDATE_QUAT_W_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_x, &protocol_buffer[AHRS_UPDATE_QUAT_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_y, &protocol_buffer[AHRS_UPDATE_QUAT_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_z, &protocol_buffer[AHRS_UPDATE_QUAT_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(baro_pressure, &protocol_buffer[AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(baro_temp_c, &protocol_buffer[AHRS_UPDATE_BARO_TEMP_VAUE_INDEX]);
+
+        protocol_buffer[AHRS_UPDATE_OPSTATUS_VALUE_INDEX] = op_status;
+        protocol_buffer[AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX] = sensor_status;
+        protocol_buffer[AHRS_UPDATE_CAL_STATUS_VALUE_INDEX] = cal_status;
+        protocol_buffer[AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX] = selftest_status;
+        // Footer
+        encodeTermination(protocol_buffer, AHRS_UPDATE_MESSAGE_LENGTH, AHRS_UPDATE_MESSAGE_LENGTH - 4);
+        return AHRS_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int decodeAHRSUpdate(char *buffer, int length, struct AHRSUpdate &update)
+    {
+        if (length < AHRS_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == AHRS_UPDATE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_AHRS_UPDATE))
+        {
+
+            if (!verifyChecksum(buffer, AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            update.yaw = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRS_UPDATE_YAW_VALUE_INDEX]);
+            update.pitch = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRS_UPDATE_PITCH_VALUE_INDEX]);
+            update.roll = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRS_UPDATE_ROLL_VALUE_INDEX]);
+            update.compass_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(&buffer[AHRS_UPDATE_HEADING_VALUE_INDEX]);
+            update.altitude = IMURegisters::decodeProtocol1616Float(&buffer[AHRS_UPDATE_ALTITUDE_VALUE_INDEX]);
+            update.fused_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(&buffer[AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+            update.linear_accel_x = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+            update.linear_accel_y = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+            update.linear_accel_z = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+            update.cal_mag_x = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX]);
+            update.cal_mag_y = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX]);
+            update.cal_mag_z = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX]);
+            update.mag_field_norm_ratio = IMURegisters::decodeProtocolUnsignedHundredthsFloat(&buffer[AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX]);
+            update.mag_field_norm_scalar = IMURegisters::decodeProtocol1616Float(&buffer[AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX]);
+            update.mpu_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+            update.raw_mag_x = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX]);
+            update.raw_mag_y = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX]);
+            update.raw_mag_z = IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX]);
+            /* AHRSPosUpdate:  Quaternions are signed int (16-bit resolution); divide by 16384 to yield +/- 2 radians */
+            update.quat_w = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_QUAT_W_VALUE_INDEX]) / 16384.0f);
+            update.quat_x = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_QUAT_X_VALUE_INDEX]) / 16384.0f);
+            update.quat_y = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_QUAT_Y_VALUE_INDEX]) / 16384.0f);
+            update.quat_z = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRS_UPDATE_QUAT_Z_VALUE_INDEX]) / 16384.0f);
+            update.barometric_pressure = IMURegisters::decodeProtocol1616Float(&buffer[AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX]);
+            update.baro_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRS_UPDATE_BARO_TEMP_VAUE_INDEX]);
+            update.op_status = buffer[AHRS_UPDATE_OPSTATUS_VALUE_INDEX];
+            update.sensor_status = buffer[AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
+            update.cal_status = buffer[AHRS_UPDATE_CAL_STATUS_VALUE_INDEX];
+            update.selftest_status = buffer[AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
+
+            return AHRS_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeAHRSPosUpdate(char *protocol_buffer,
+                                   float yaw, float pitch, float roll,
+                                   float compass_heading, float altitude, float fused_heading,
+                                   float linear_accel_x, float linear_accel_y, float linear_accel_z,
+                                   float mpu_temp_c,
+                                   int16_t quat_w, int16_t quat_x, int16_t quat_y, int16_t quat_z,
+                                   float vel_x, float vel_y, float vel_z,
+                                   float disp_x, float disp_y, float disp_z,
+                                   uint8_t op_status, uint8_t sensor_status,
+                                   uint8_t cal_status, uint8_t selftest_status)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = AHRSPOS_UPDATE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_AHRSPOS_UPDATE;
+        // data
+        IMURegisters::encodeProtocolSignedHundredthsFloat(yaw, &protocol_buffer[AHRSPOS_UPDATE_YAW_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(pitch, &protocol_buffer[AHRSPOS_UPDATE_PITCH_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(roll, &protocol_buffer[AHRSPOS_UPDATE_ROLL_VALUE_INDEX]);
+        IMURegisters::encodeProtocolUnsignedHundredthsFloat(compass_heading, &protocol_buffer[AHRSPOS_UPDATE_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(altitude, &protocol_buffer[AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX]);
+        IMURegisters::encodeProtocolUnsignedHundredthsFloat(fused_heading, &protocol_buffer[AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_x, &protocol_buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_y, &protocol_buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedThousandthsFloat(linear_accel_z, &protocol_buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_x, &protocol_buffer[AHRSPOS_UPDATE_VEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_y, &protocol_buffer[AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_z, &protocol_buffer[AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_x, &protocol_buffer[AHRSPOS_UPDATE_DISP_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_y, &protocol_buffer[AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_z, &protocol_buffer[AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(mpu_temp_c, &protocol_buffer[AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_w, &protocol_buffer[AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_x, &protocol_buffer[AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_y, &protocol_buffer[AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(quat_z, &protocol_buffer[AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX]);
+
+        protocol_buffer[AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX] = op_status;
+        protocol_buffer[AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX] = sensor_status;
+        protocol_buffer[AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX] = cal_status;
+        protocol_buffer[AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX] = selftest_status;
+        // Footer
+        encodeTermination(protocol_buffer, AHRSPOS_UPDATE_MESSAGE_LENGTH, AHRSPOS_UPDATE_MESSAGE_LENGTH - 4);
+        return AHRSPOS_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int decodeAHRSPosUpdate(char *buffer, int length, struct AHRSPosUpdate &update)
+    {
+        if (length < AHRSPOS_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == AHRSPOS_UPDATE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_AHRSPOS_UPDATE))
+        {
+
+            if (!verifyChecksum(buffer, AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            update.yaw = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_YAW_VALUE_INDEX]);
+            update.pitch = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_PITCH_VALUE_INDEX]);
+            update.roll = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_ROLL_VALUE_INDEX]);
+            update.compass_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_HEADING_VALUE_INDEX]);
+            update.altitude = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX]);
+            update.fused_heading = IMURegisters::decodeProtocolUnsignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+            update.linear_accel_x = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+            update.linear_accel_y = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+            update.linear_accel_z = IMURegisters::decodeProtocolSignedThousandthsFloat(&buffer[AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+            update.vel_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_VEL_X_VALUE_INDEX]);
+            update.vel_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX]);
+            update.vel_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX]);
+            update.disp_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_DISP_X_VALUE_INDEX]);
+            update.disp_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX]);
+            update.disp_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX]);
+            update.mpu_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+            /* AHRSPosUpdate:  Quaternions are signed int (16-bit resolution); divide by 16384 to yield +/- 2 radians */
+            update.quat_w = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX]) / 16384.0f);
+            update.quat_x = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX]) / 16384.0f);
+            update.quat_y = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX]) / 16384.0f);
+            update.quat_z = ((float)IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX]) / 16384.0f);
+            update.op_status = buffer[AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX];
+            update.sensor_status = buffer[AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
+            update.cal_status = buffer[AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX];
+            update.selftest_status = buffer[AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
+
+            return AHRSPOS_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeAHRSPosTSUpdate(char *protocol_buffer,
+                                     float yaw, float pitch, float roll,
+                                     float compass_heading, float altitude, float fused_heading,
+                                     float linear_accel_x, float linear_accel_y, float linear_accel_z,
+                                     float mpu_temp_c,
+                                     float quat_w, float quat_x, float quat_y, float quat_z,
+                                     float vel_x, float vel_y, float vel_z,
+                                     float disp_x, float disp_y, float disp_z,
+                                     uint8_t op_status, uint8_t sensor_status,
+                                     uint8_t cal_status, uint8_t selftest_status,
+                                     uint32_t timestamp)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = AHRSPOS_TS_UPDATE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_AHRSPOS_TS_UPDATE;
+
+        // data
+        IMURegisters::encodeProtocol1616Float(yaw, &protocol_buffer[AHRSPOS_TS_UPDATE_YAW_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(pitch, &protocol_buffer[AHRSPOS_TS_UPDATE_PITCH_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(roll, &protocol_buffer[AHRSPOS_TS_UPDATE_ROLL_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(compass_heading, &protocol_buffer[AHRSPOS_TS_UPDATE_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(altitude, &protocol_buffer[AHRSPOS_TS_UPDATE_ALTITUDE_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(fused_heading, &protocol_buffer[AHRSPOS_TS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_x, &protocol_buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_y, &protocol_buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_z, &protocol_buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_x, &protocol_buffer[AHRSPOS_TS_UPDATE_VEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_y, &protocol_buffer[AHRSPOS_TS_UPDATE_VEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_z, &protocol_buffer[AHRSPOS_TS_UPDATE_VEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_x, &protocol_buffer[AHRSPOS_TS_UPDATE_DISP_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_y, &protocol_buffer[AHRSPOS_TS_UPDATE_DISP_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_z, &protocol_buffer[AHRSPOS_TS_UPDATE_DISP_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(mpu_temp_c, &protocol_buffer[AHRSPOS_TS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_w, &protocol_buffer[AHRSPOS_TS_UPDATE_QUAT_W_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_x, &protocol_buffer[AHRSPOS_TS_UPDATE_QUAT_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_y, &protocol_buffer[AHRSPOS_TS_UPDATE_QUAT_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_z, &protocol_buffer[AHRSPOS_TS_UPDATE_QUAT_Z_VALUE_INDEX]);
+
+        protocol_buffer[AHRSPOS_TS_UPDATE_OPSTATUS_VALUE_INDEX] = op_status;
+        protocol_buffer[AHRSPOS_TS_UPDATE_SENSOR_STATUS_VALUE_INDEX] = sensor_status;
+        protocol_buffer[AHRSPOS_TS_UPDATE_CAL_STATUS_VALUE_INDEX] = cal_status;
+        protocol_buffer[AHRSPOS_TS_UPDATE_SELFTEST_STATUS_VALUE_INDEX] = selftest_status;
+        IMURegisters::encodeProtocolInt32((int32_t)timestamp, &protocol_buffer[AHRSPOS_TS_UPDATE_TIMESTAMP_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, AHRSPOS_TS_UPDATE_MESSAGE_LENGTH, AHRSPOS_TS_UPDATE_MESSAGE_LENGTH - 4);
+        return AHRSPOS_TS_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int decodeAHRSPosTSUpdate(char *buffer, int length, struct AHRSPosTSUpdate &update)
+    {
+        if (length < AHRSPOS_TS_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == AHRSPOS_TS_UPDATE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_AHRSPOS_TS_UPDATE))
+        {
+
+            if (!verifyChecksum(buffer, AHRSPOS_TS_UPDATE_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            update.yaw = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_YAW_VALUE_INDEX]);
+            update.pitch = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_PITCH_VALUE_INDEX]);
+            update.roll = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_ROLL_VALUE_INDEX]);
+            update.compass_heading = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_HEADING_VALUE_INDEX]);
+            update.altitude = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_ALTITUDE_VALUE_INDEX]);
+            update.fused_heading = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+            update.linear_accel_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+            update.linear_accel_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+            update.linear_accel_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+            update.vel_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_VEL_X_VALUE_INDEX]);
+            update.vel_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_VEL_Y_VALUE_INDEX]);
+            update.vel_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_VEL_Z_VALUE_INDEX]);
+            update.disp_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_DISP_X_VALUE_INDEX]);
+            update.disp_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_DISP_Y_VALUE_INDEX]);
+            update.disp_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_DISP_Z_VALUE_INDEX]);
+            update.mpu_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_TS_UPDATE_MPU_TEMP_VAUE_INDEX]);
+            update.quat_w = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_QUAT_W_VALUE_INDEX]) / 16384.0f;
+            update.quat_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_QUAT_X_VALUE_INDEX]) / 16384.0f;
+            update.quat_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_QUAT_Y_VALUE_INDEX]) / 16384.0f;
+            update.quat_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_UPDATE_QUAT_Z_VALUE_INDEX]) / 16384.0f;
+            update.op_status = buffer[AHRSPOS_TS_UPDATE_OPSTATUS_VALUE_INDEX];
+            update.sensor_status = buffer[AHRSPOS_TS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
+            update.cal_status = buffer[AHRSPOS_TS_UPDATE_CAL_STATUS_VALUE_INDEX];
+            update.selftest_status = buffer[AHRSPOS_TS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
+            update.timestamp = (uint32_t)IMURegisters::decodeProtocolInt32(&buffer[AHRSPOS_TS_UPDATE_TIMESTAMP_INDEX]);
+
+            return AHRSPOS_TS_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeAHRSPosTSRawUpdate(char *protocol_buffer,
+                                        float yaw, float pitch, float roll,
+                                        float compass_heading, float altitude, float fused_heading,
+                                        float linear_accel_x, float linear_accel_y, float linear_accel_z,
+                                        float mpu_temp_c,
+                                        float quat_w, float quat_x, float quat_y, float quat_z,
+                                        float vel_x, float vel_y, float vel_z,
+                                        float disp_x, float disp_y, float disp_z,
+                                        int16_t raw_gyro_x, int16_t raw_gyro_y, int16_t raw_gyro_z,
+                                        int16_t raw_acc_x, int16_t raw_acc_y, int16_t raw_acc_z,
+                                        float delta_t_sec,
+                                        bool gyro_bias_updated,
+                                        uint8_t op_status, uint8_t sensor_status,
+                                        uint8_t cal_status, uint8_t selftest_status,
+                                        uint32_t timestamp)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_AHRSPOS_TS_RAW_UPDATE;
+
+        // data
+        IMURegisters::encodeProtocol1616Float(yaw, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_YAW_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(pitch, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_PITCH_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(roll, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_ROLL_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(compass_heading, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(altitude, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_ALTITUDE_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(fused_heading, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(linear_accel_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_VEL_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_VEL_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(vel_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_VEL_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_DISP_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_DISP_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(disp_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_DISP_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolSignedHundredthsFloat(mpu_temp_c, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_MPU_TEMP_VAUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_w, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_W_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(quat_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_Z_VALUE_INDEX]);
+
+        protocol_buffer[AHRSPOS_TS_RAW_UPDATE_OPSTATUS_VALUE_INDEX] = op_status;
+        protocol_buffer[AHRSPOS_TS_RAW_UPDATE_SENSOR_STATUS_VALUE_INDEX] = sensor_status;
+        protocol_buffer[AHRSPOS_TS_RAW_UPDATE_CAL_STATUS_VALUE_INDEX] = cal_status;
+        protocol_buffer[AHRSPOS_TS_RAW_UPDATE_SELFTEST_STATUS_VALUE_INDEX] = selftest_status;
+        IMURegisters::encodeProtocolInt32((int32_t)timestamp, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_TIMESTAMP_INDEX]);
+
+        IMURegisters::encodeProtocolInt16(raw_gyro_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_gyro_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_gyro_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_acc_x, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_ACC_X_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_acc_y, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_ACC_Y_VALUE_INDEX]);
+        IMURegisters::encodeProtocolInt16(raw_acc_z, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_ACC_Z_VALUE_INDEX]);
+        IMURegisters::encodeProtocol1616Float(delta_t_sec, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_LAST_SAMPLE_DELTA_SEC_INDEX]);
+        IMURegisters::encodeProtocolInt16((int16_t)gyro_bias_updated, &protocol_buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_BIAS_UPDATED_BOOL_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH, AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH - 4);
+        return AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int decodeAHRSPosTSRawUpdate(char *buffer, int length, struct AHRSPosTSRawUpdate &update)
+    {
+        if (length < AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_AHRSPOS_TS_RAW_UPDATE))
+        {
+
+            if (!verifyChecksum(buffer, AHRSPOS_TS_RAW_UPDATE_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            update.yaw = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_YAW_VALUE_INDEX]);
+            update.pitch = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_PITCH_VALUE_INDEX]);
+            update.roll = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_ROLL_VALUE_INDEX]);
+            update.compass_heading = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_HEADING_VALUE_INDEX]);
+            update.altitude = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_ALTITUDE_VALUE_INDEX]);
+            update.fused_heading = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_FUSED_HEADING_VALUE_INDEX]);
+            update.linear_accel_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX]);
+            update.linear_accel_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX]);
+            update.linear_accel_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX]);
+            update.vel_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_VEL_X_VALUE_INDEX]);
+            update.vel_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_VEL_Y_VALUE_INDEX]);
+            update.vel_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_VEL_Z_VALUE_INDEX]);
+            update.disp_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_DISP_X_VALUE_INDEX]);
+            update.disp_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_DISP_Y_VALUE_INDEX]);
+            update.disp_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_DISP_Z_VALUE_INDEX]);
+            update.mpu_temp = IMURegisters::decodeProtocolSignedHundredthsFloat(&buffer[AHRSPOS_TS_RAW_UPDATE_MPU_TEMP_VAUE_INDEX]);
+            update.quat_w = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_W_VALUE_INDEX]) / 16384.0f;
+            update.quat_x = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_X_VALUE_INDEX]) / 16384.0f;
+            update.quat_y = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_Y_VALUE_INDEX]) / 16384.0f;
+            update.quat_z = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_QUAT_Z_VALUE_INDEX]) / 16384.0f;
+            update.op_status = buffer[AHRSPOS_TS_RAW_UPDATE_OPSTATUS_VALUE_INDEX];
+            update.sensor_status = buffer[AHRSPOS_TS_RAW_UPDATE_SENSOR_STATUS_VALUE_INDEX];
+            update.cal_status = buffer[AHRSPOS_TS_RAW_UPDATE_CAL_STATUS_VALUE_INDEX];
+            update.selftest_status = buffer[AHRSPOS_TS_RAW_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
+            update.timestamp = (uint32_t)IMURegisters::decodeProtocolInt32(&buffer[AHRSPOS_TS_RAW_UPDATE_TIMESTAMP_INDEX]);
+
+            update.raw_gyro_x = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_X_VALUE_INDEX]);
+            update.raw_gyro_y = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_Y_VALUE_INDEX]);
+            update.raw_gyro_z = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_Z_VALUE_INDEX]);
+            update.accel_x = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_ACC_X_VALUE_INDEX]);
+            update.accel_y = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_ACC_Y_VALUE_INDEX]);
+            update.accel_z = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_ACC_Z_VALUE_INDEX]);
+            update.delta_t_sec = IMURegisters::decodeProtocol1616Float(&buffer[AHRSPOS_TS_RAW_UPDATE_LAST_SAMPLE_DELTA_SEC_INDEX]);
+            update.gyro_bias_updated = IMURegisters::decodeProtocolInt16(&buffer[AHRSPOS_TS_RAW_UPDATE_GYRO_BIAS_UPDATED_BOOL_INDEX]) != 0;
+            return AHRSPOS_TS_RAW_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeMagCalCommand(char *protocol_buffer, AHRS_DATA_ACTION action, int16_t *bias, float *matrix, float earth_mag_field_norm)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = MAG_CAL_CMD_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_MAG_CAL_CMD;
+
+        // Data
+        protocol_buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX] = action;
+        for (int i = 0; i < 3; i++)
+        {
+            IMURegisters::encodeProtocolInt16(bias[i],
+                                              &protocol_buffer[MAG_X_BIAS_VALUE_INDEX + (i * sizeof(int16_t))]);
+        }
+        for (int i = 0; i < 9; i++)
+        {
+            IMURegisters::encodeProtocol1616Float(matrix[i], &protocol_buffer[MAG_XFORM_1_1_VALUE_INDEX + (i * sizeof(s_1616_float))]);
+        }
+        IMURegisters::encodeProtocol1616Float(earth_mag_field_norm, &protocol_buffer[MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX]);
+        // Footer
+        encodeTermination(protocol_buffer, MAG_CAL_CMD_MESSAGE_LENGTH, MAG_CAL_CMD_MESSAGE_LENGTH - 4);
+        return MAG_CAL_CMD_MESSAGE_LENGTH;
+    }
+
+    static int decodeMagCalCommand(char *buffer, int length,
+                                   AHRS_DATA_ACTION &action,
+                                   int16_t *bias,
+                                   float *matrix,
+                                   float &earth_mag_field_norm)
+    {
+        if (length < MAG_CAL_CMD_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == MAG_CAL_CMD_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_MAG_CAL_CMD))
+        {
+
+            if (!verifyChecksum(buffer, MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            action = (AHRS_DATA_ACTION)buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX];
+            for (int i = 0; i < 3; i++)
+            {
+                bias[i] = IMURegisters::decodeProtocolInt16(&buffer[MAG_X_BIAS_VALUE_INDEX + (i * sizeof(int16_t))]);
+            }
+            for (int i = 0; i < 9; i++)
+            {
+                matrix[i] = IMURegisters::decodeProtocol1616Float(&buffer[MAG_XFORM_1_1_VALUE_INDEX + (i * sizeof(s_1616_float))]);
+            }
+            earth_mag_field_norm = IMURegisters::decodeProtocol1616Float(&buffer[MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX]);
+            return MAG_CAL_CMD_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeDataSetResponse(char *protocol_buffer, AHRS_DATA_TYPE type, AHRS_TUNING_VAR_ID subtype, uint8_t status)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = DATA_SET_RESPONSE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_DATA_SET_RESPONSE;
+        // Data
+        protocol_buffer[DATA_SET_RESPONSE_DATATYPE_VALUE_INDEX] = type;
+        protocol_buffer[DATA_SET_RESPONSE_VARID_VALUE_INDEX] = subtype;
+        protocol_buffer[DATA_SET_RESPONSE_STATUS_VALUE_INDEX] = status;
+        // Footer
+        encodeTermination(protocol_buffer, DATA_SET_RESPONSE_MESSAGE_LENGTH, DATA_SET_RESPONSE_MESSAGE_LENGTH - 4);
+        return DATA_SET_RESPONSE_MESSAGE_LENGTH;
+    }
+
+    static int decodeDataSetResponse(char *buffer, int length, AHRS_DATA_TYPE &type, AHRS_TUNING_VAR_ID &subtype, uint8_t &status)
+    {
+        if (length < DATA_SET_RESPONSE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == DATA_SET_RESPONSE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_DATA_SET_RESPONSE))
+        {
+
+            if (!verifyChecksum(buffer, DATA_SET_RESPONSE_MESSAGE_CHECKSUM_INDEX))
+                return 0;
+
+            type = (AHRS_DATA_TYPE)buffer[DATA_SET_RESPONSE_DATATYPE_VALUE_INDEX];
+            subtype = (AHRS_TUNING_VAR_ID)buffer[DATA_SET_RESPONSE_VARID_VALUE_INDEX];
+            status = buffer[DATA_SET_RESPONSE_STATUS_VALUE_INDEX];
+            return DATA_SET_RESPONSE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeDataGetRequest(char *protocol_buffer, AHRS_DATA_TYPE type, AHRS_TUNING_VAR_ID subtype)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = DATA_REQUEST_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_DATA_REQUEST;
+        // Data
+        protocol_buffer[DATA_REQUEST_DATATYPE_VALUE_INDEX] = type;
+        protocol_buffer[DATA_REQUEST_VARIABLEID_VALUE_INDEX] = subtype;
+        // Footer
+        encodeTermination(protocol_buffer, DATA_REQUEST_MESSAGE_LENGTH, DATA_REQUEST_MESSAGE_LENGTH - 4);
+        return DATA_REQUEST_MESSAGE_LENGTH;
+    }
+
+    static int decodeDataGetRequest(char *buffer, int length, AHRS_DATA_TYPE &type, AHRS_TUNING_VAR_ID &subtype)
+    {
+        if (length < DATA_REQUEST_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == DATA_REQUEST_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_DATA_REQUEST))
+        {
+
+            if (!verifyChecksum(buffer, DATA_REQUEST_CHECKSUM_INDEX))
+                return 0;
+
+            type = (AHRS_DATA_TYPE)buffer[DATA_REQUEST_DATATYPE_VALUE_INDEX];
+            subtype = (AHRS_TUNING_VAR_ID)buffer[DATA_REQUEST_VARIABLEID_VALUE_INDEX];
+
+            return DATA_REQUEST_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int encodeBoardIdentityResponse(char *protocol_buffer, uint8_t type, uint8_t fw_rev,
+                                           uint8_t fw_ver_major, uint8_t fw_ver_minor, uint16_t fw_revision,
+                                           uint8_t *unique_id)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = BINARY_PACKET_INDICATOR_CHAR;
+        protocol_buffer[2] = BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH - 2;
+        protocol_buffer[3] = MSGID_BOARD_IDENTITY_RESPONSE;
+        // Data
+        protocol_buffer[BOARD_IDENTITY_BOARDTYPE_VALUE_INDEX] = type;
+        protocol_buffer[BOARD_IDENTITY_HWREV_VALUE_INDEX] = fw_rev;
+        protocol_buffer[BOARD_IDENTITY_FW_VER_MAJOR] = fw_ver_major;
+        protocol_buffer[BOARD_IDENTITY_FW_VER_MINOR] = fw_ver_minor;
+        IMURegisters::encodeProtocolUint16(fw_revision, &protocol_buffer[BOARD_IDENTITY_FW_VER_REVISION_VALUE_INDEX]);
+        for (int i = 0; i < 12; i++)
+        {
+            protocol_buffer[BOARD_IDENTITY_UNIQUE_ID_0 + i] = unique_id[i];
+        }
+        // Footer
+        encodeTermination(protocol_buffer, BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH, BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH - 4);
+        return BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH;
+    }
+
+    static int decodeBoardIdentityResponse(char *buffer, int length, struct BoardID &update)
+    {
+        if (length < BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) &&
+            (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
+            (buffer[2] == BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH - 2) &&
+            (buffer[3] == MSGID_BOARD_IDENTITY_RESPONSE))
+        {
+            if (!verifyChecksum(buffer, BOARD_IDENTITY_RESPONSE_CHECKSUM_INDEX))
+                return 0;
+
+            update.type = buffer[BOARD_IDENTITY_BOARDTYPE_VALUE_INDEX];
+            update.hw_rev = buffer[BOARD_IDENTITY_HWREV_VALUE_INDEX];
+            update.fw_ver_major = buffer[BOARD_IDENTITY_FW_VER_MAJOR];
+            update.fw_ver_minor = buffer[BOARD_IDENTITY_FW_VER_MINOR];
+            update.fw_revision = IMURegisters::decodeProtocolUint16(&buffer[BOARD_IDENTITY_FW_VER_REVISION_VALUE_INDEX]);
+            for (int i = 0; i < 12; i++)
+            {
+                update.unique_id[i] = buffer[BOARD_IDENTITY_UNIQUE_ID_0 + i];
+            }
+            return BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+};
+
+#endif // _IMU_PROTOCOL_H_
diff --git a/src/main/native/include/IMUProtocol.h b/src/main/native/include/IMUProtocol.h
new file mode 100644
index 0000000..3c7e64a
--- /dev/null
+++ b/src/main/native/include/IMUProtocol.h
@@ -0,0 +1,512 @@
+/* ============================================
+Nav6 source code is placed under the MIT license
+Copyright (c) 2013 Kauai Labs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+ */
+
+#ifndef _IMU_PROTOCOL_H_
+#define _IMU_PROTOCOL_H_
+
+#define PACKET_START_CHAR '!'
+#define CHECKSUM_LENGTH 2   /* 8-bit checksump, all bytes before checksum */
+#define TERMINATOR_LENGTH 2 /* Carriage Return, Line Feed */
+
+#define PROTOCOL_FLOAT_LENGTH 7
+
+// Yaw/Pitch/Roll (YPR) Update Packet - e.g., !y[yaw][pitch][roll][compass]
+// (All values as floats)
+
+#define MSGID_YPR_UPDATE 'y'
+#define YPR_UPDATE_YAW_VALUE_INDEX 2
+#define YPR_UPDATE_ROLL_VALUE_INDEX 9
+#define YPR_UPDATE_PITCH_VALUE_INDEX 16
+#define YPR_UPDATE_COMPASS_VALUE_INDEX 23
+#define YPR_UPDATE_CHECKSUM_INDEX 30
+#define YPR_UPDATE_TERMINATOR_INDEX 32
+#define YPR_UPDATE_MESSAGE_LENGTH 34
+
+// Quaternion Update Packet - e.g., !r[q1][q2][q3][q4][accelx][accely][accelz][magx][magy][magz]
+
+#define MSGID_QUATERNION_UPDATE 'q'
+#define QUATERNION_UPDATE_QUAT1_VALUE_INDEX 2
+#define QUATERNION_UPDATE_QUAT2_VALUE_INDEX 6
+#define QUATERNION_UPDATE_QUAT3_VALUE_INDEX 10
+#define QUATERNION_UPDATE_QUAT4_VALUE_INDEX 14
+#define QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX 18
+#define QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX 22
+#define QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX 26
+#define QUATERNION_UPDATE_MAG_X_VALUE_INDEX 30
+#define QUATERNION_UPDATE_MAG_Y_VALUE_INDEX 34
+#define QUATERNION_UPDATE_MAG_Z_VALUE_INDEX 38
+#define QUATERNION_UPDATE_TEMP_VALUE_INDEX 42
+#define QUATERNION_UPDATE_CHECKSUM_INDEX 49
+#define QUATERNION_UPDATE_TERMINATOR_INDEX 51
+#define QUATERNION_UPDATE_MESSAGE_LENGTH 53
+
+// Gyro/Raw Data Update packet - e.g., !g[gx][gy][gz][accelx][accely][accelz][magx][magy][magz][temp_c]
+
+#define MSGID_GYRO_UPDATE 'g'
+#define GYRO_UPDATE_MESSAGE_LENGTH 46
+#define GYRO_UPDATE_GYRO_X_VALUE_INDEX 2
+#define GYRO_UPDATE_GYRO_Y_VALUE_INDEX 6
+#define GYRO_UPDATE_GYRO_Z_VALUE_INDEX 10
+#define GYRO_UPDATE_ACCEL_X_VALUE_INDEX 14
+#define GYRO_UPDATE_ACCEL_Y_VALUE_INDEX 18
+#define GYRO_UPDATE_ACCEL_Z_VALUE_INDEX 22
+#define GYRO_UPDATE_MAG_X_VALUE_INDEX 26
+#define GYRO_UPDATE_MAG_Y_VALUE_INDEX 30
+#define GYRO_UPDATE_MAG_Z_VALUE_INDEX 34
+#define GYRO_UPDATE_TEMP_VALUE_INDEX 38
+#define GYRO_UPDATE_CHECKSUM_INDEX 42
+#define GYRO_UPDATE_TERMINATOR_INDEX 44
+
+// EnableStream Command Packet - e.g., !S[stream type]
+
+#define MSGID_STREAM_CMD 'S'
+#define STREAM_CMD_STREAM_TYPE_YPR MSGID_YPR_UPDATE
+#define STREAM_CMD_STREAM_TYPE_QUATERNION MSGID_QUATERNION_UPDATE
+#define STREAM_CMD_STREAM_TYPE_RAW MSGID_RAW_UPDATE
+#define STREAM_CMD_STREAM_TYPE_INDEX 2
+#define STREAM_CMD_UPDATE_RATE_HZ_INDEX 3
+#define STREAM_CMD_CHECKSUM_INDEX 5
+#define STREAM_CMD_TERMINATOR_INDEX 7
+#define STREAM_CMD_MESSAGE_LENGTH 9
+
+// EnableStream Response Packet - e.g., !s[stream type][gyro full scale range][accel full scale range][update rate hz][yaw_offset_degrees][q1/2/3/4 offsets][flags][checksum][cr][lf]
+#define MSG_ID_STREAM_RESPONSE 's'
+#define STREAM_RESPONSE_STREAM_TYPE_INDEX 2
+#define STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE 3
+#define STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE 7
+#define STREAM_RESPONSE_UPDATE_RATE_HZ 11
+#define STREAM_RESPONSE_YAW_OFFSET_DEGREES 15
+#define STREAM_RESPONSE_QUAT1_OFFSET 22 /* Deprecated */
+#define STREAM_RESPONSE_QUAT2_OFFSET 26 /* Deprecated */
+#define STREAM_RESPONSE_QUAT3_OFFSET 30 /* Deprecated */
+#define STREAM_RESPONSE_QUAT4_OFFSET 34 /* Deprecated */
+#define STREAM_RESPONSE_FLAGS 38
+#define STREAM_RESPONSE_CHECKSUM_INDEX 42
+#define STREAM_RESPONSE_TERMINATOR_INDEX 44
+#define STREAM_RESPONSE_MESSAGE_LENGTH 46
+
+#define NAV6_FLAG_MASK_CALIBRATION_STATE 0x03
+#define NAV6_CALIBRATION_STATE_WAIT 0x00
+#define NAV6_CALIBRATION_STATE_ACCUMULATE 0x01
+#define NAV6_CALIBRATION_STATE_COMPLETE 0x02
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#define IMU_PROTOCOL_MAX_MESSAGE_LENGTH QUATERNION_UPDATE_MESSAGE_LENGTH
+
+class IMUProtocol
+{
+public:
+    struct YPRUpdate
+    {
+        float yaw;
+        float pitch;
+        float roll;
+        float compass_heading;
+    };
+
+    struct GyroUpdate
+    {
+        int16_t gyro_x;
+        int16_t gyro_y;
+        int16_t gyro_z;
+        int16_t accel_x;
+        int16_t accel_y;
+        int16_t accel_z;
+        int16_t mag_x;
+        int16_t mag_y;
+        int16_t mag_z;
+        float temp_c;
+    };
+
+    struct QuaternionUpdate
+    {
+        int16_t q1;
+        int16_t q2;
+        int16_t q3;
+        int16_t q4;
+        int16_t accel_x;
+        int16_t accel_y;
+        int16_t accel_z;
+        int16_t mag_x;
+        int16_t mag_y;
+        int16_t mag_z;
+        float temp_c;
+    };
+
+    struct StreamResponse
+    {
+        uint8_t stream_type;
+        int16_t gyro_fsr_dps;
+        int16_t accel_fsr_g;
+        int16_t update_rate_hz;
+        float yaw_offset_degrees;
+        int16_t q1_offset;
+        int16_t q2_offset;
+        int16_t q3_offset;
+        int16_t q4_offset;
+        int16_t flags;
+    };
+
+public:
+    static int encodeYPRUpdate(char *protocol_buffer, float yaw, float pitch, float roll, float compass_heading)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = MSGID_YPR_UPDATE;
+
+        // Data
+        encodeProtocolFloat(yaw, &protocol_buffer[YPR_UPDATE_YAW_VALUE_INDEX]);
+        encodeProtocolFloat(pitch, &protocol_buffer[YPR_UPDATE_PITCH_VALUE_INDEX]);
+        encodeProtocolFloat(roll, &protocol_buffer[YPR_UPDATE_ROLL_VALUE_INDEX]);
+        encodeProtocolFloat(compass_heading, &protocol_buffer[YPR_UPDATE_COMPASS_VALUE_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, YPR_UPDATE_MESSAGE_LENGTH, YPR_UPDATE_MESSAGE_LENGTH - 4);
+
+        return YPR_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int encodeQuaternionUpdate(char *protocol_buffer,
+                                      uint16_t q1, uint16_t q2, uint16_t q3, uint16_t q4,
+                                      uint16_t accel_x, uint16_t accel_y, uint16_t accel_z,
+                                      int16_t mag_x, int16_t mag_y, int16_t mag_z,
+                                      float temp_c)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = MSGID_QUATERNION_UPDATE;
+
+        // Data
+        encodeProtocolUint16(q1, &protocol_buffer[QUATERNION_UPDATE_QUAT1_VALUE_INDEX]);
+        encodeProtocolUint16(q2, &protocol_buffer[QUATERNION_UPDATE_QUAT2_VALUE_INDEX]);
+        encodeProtocolUint16(q3, &protocol_buffer[QUATERNION_UPDATE_QUAT3_VALUE_INDEX]);
+        encodeProtocolUint16(q4, &protocol_buffer[QUATERNION_UPDATE_QUAT4_VALUE_INDEX]);
+        encodeProtocolUint16(accel_x, &protocol_buffer[QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX]);
+        encodeProtocolUint16(accel_y, &protocol_buffer[QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX]);
+        encodeProtocolUint16(accel_z, &protocol_buffer[QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_x, &protocol_buffer[QUATERNION_UPDATE_MAG_X_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_y, &protocol_buffer[QUATERNION_UPDATE_MAG_Y_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_z, &protocol_buffer[QUATERNION_UPDATE_MAG_Z_VALUE_INDEX]);
+        encodeProtocolFloat(temp_c, &protocol_buffer[QUATERNION_UPDATE_TEMP_VALUE_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, QUATERNION_UPDATE_MESSAGE_LENGTH, QUATERNION_UPDATE_MESSAGE_LENGTH - 4);
+
+        return QUATERNION_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int encodeGyroUpdate(char *protocol_buffer,
+                                uint16_t gyro_x, uint16_t gyro_y, uint16_t gyro_z,
+                                uint16_t accel_x, uint16_t accel_y, uint16_t accel_z,
+                                int16_t mag_x, int16_t mag_y, int16_t mag_z,
+                                float temp_c)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = MSGID_GYRO_UPDATE;
+
+        // Data
+        encodeProtocolUint16(gyro_x, &protocol_buffer[GYRO_UPDATE_GYRO_X_VALUE_INDEX]);
+        encodeProtocolUint16(gyro_y, &protocol_buffer[GYRO_UPDATE_GYRO_Y_VALUE_INDEX]);
+        encodeProtocolUint16(gyro_z, &protocol_buffer[GYRO_UPDATE_GYRO_Z_VALUE_INDEX]);
+        encodeProtocolUint16(accel_x, &protocol_buffer[GYRO_UPDATE_ACCEL_X_VALUE_INDEX]);
+        encodeProtocolUint16(accel_y, &protocol_buffer[GYRO_UPDATE_ACCEL_Y_VALUE_INDEX]);
+        encodeProtocolUint16(accel_z, &protocol_buffer[GYRO_UPDATE_ACCEL_Z_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_x, &protocol_buffer[GYRO_UPDATE_MAG_X_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_y, &protocol_buffer[GYRO_UPDATE_MAG_Y_VALUE_INDEX]);
+        encodeProtocolUint16((uint16_t)mag_z, &protocol_buffer[GYRO_UPDATE_MAG_Z_VALUE_INDEX]);
+        encodeProtocolUnsignedHundredthsFloat(temp_c, &protocol_buffer[GYRO_UPDATE_TEMP_VALUE_INDEX]);
+
+        // Footer
+        encodeTermination(protocol_buffer, GYRO_UPDATE_MESSAGE_LENGTH, GYRO_UPDATE_MESSAGE_LENGTH - 4);
+
+        return GYRO_UPDATE_MESSAGE_LENGTH;
+    }
+
+    static int encodeStreamCommand(char *protocol_buffer, char stream_type, unsigned char update_rate_hz)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = MSGID_STREAM_CMD;
+
+        // Data
+        protocol_buffer[STREAM_CMD_STREAM_TYPE_INDEX] = stream_type;
+        // convert update_rate_hz to two ascii bytes
+        sprintf(&protocol_buffer[STREAM_CMD_UPDATE_RATE_HZ_INDEX], "%02X", update_rate_hz);
+
+        // Footer
+        encodeTermination(protocol_buffer, STREAM_CMD_MESSAGE_LENGTH, STREAM_CMD_MESSAGE_LENGTH - 4);
+
+        return STREAM_CMD_MESSAGE_LENGTH;
+    }
+
+    static int encodeStreamResponse(char *protocol_buffer, char stream_type,
+                                    uint16_t gyro_fsr_dps, uint16_t accel_fsr_g, uint16_t update_rate_hz,
+                                    float yaw_offset_degrees,
+                                    uint16_t q1_offset, uint16_t q2_offset, uint16_t q3_offset, uint16_t q4_offset,
+                                    uint16_t flags)
+    {
+        // Header
+        protocol_buffer[0] = PACKET_START_CHAR;
+        protocol_buffer[1] = MSG_ID_STREAM_RESPONSE;
+
+        // Data
+        protocol_buffer[STREAM_RESPONSE_STREAM_TYPE_INDEX] = stream_type;
+        encodeProtocolUint16(gyro_fsr_dps, &protocol_buffer[STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE]);
+        encodeProtocolUint16(accel_fsr_g, &protocol_buffer[STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE]);
+        encodeProtocolUint16(update_rate_hz, &protocol_buffer[STREAM_RESPONSE_UPDATE_RATE_HZ]);
+        encodeProtocolFloat(yaw_offset_degrees, &protocol_buffer[STREAM_RESPONSE_YAW_OFFSET_DEGREES]);
+        encodeProtocolUint16(q1_offset, &protocol_buffer[STREAM_RESPONSE_QUAT1_OFFSET]);
+        encodeProtocolUint16(q2_offset, &protocol_buffer[STREAM_RESPONSE_QUAT2_OFFSET]);
+        encodeProtocolUint16(q3_offset, &protocol_buffer[STREAM_RESPONSE_QUAT3_OFFSET]);
+        encodeProtocolUint16(q4_offset, &protocol_buffer[STREAM_RESPONSE_QUAT4_OFFSET]);
+        encodeProtocolUint16(flags, &protocol_buffer[STREAM_RESPONSE_FLAGS]);
+
+        // Footer
+        encodeTermination(protocol_buffer, STREAM_RESPONSE_MESSAGE_LENGTH, STREAM_RESPONSE_MESSAGE_LENGTH - 4);
+
+        return STREAM_RESPONSE_MESSAGE_LENGTH;
+    }
+
+    static int decodeStreamResponse(char *buffer, int length, struct StreamResponse &rsp)
+    {
+        if (length < STREAM_RESPONSE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) && (buffer[1] == MSG_ID_STREAM_RESPONSE))
+        {
+            if (!verifyChecksum(buffer, STREAM_RESPONSE_CHECKSUM_INDEX))
+                return 0;
+
+            rsp.stream_type = buffer[2];
+            rsp.gyro_fsr_dps = decodeProtocolUint16(&buffer[STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE]);
+            rsp.accel_fsr_g = decodeProtocolUint16(&buffer[STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE]);
+            rsp.update_rate_hz = decodeProtocolUint16(&buffer[STREAM_RESPONSE_UPDATE_RATE_HZ]);
+            rsp.yaw_offset_degrees = decodeProtocolFloat(&buffer[STREAM_RESPONSE_YAW_OFFSET_DEGREES]);
+            rsp.q1_offset = decodeProtocolUint16(&buffer[STREAM_RESPONSE_QUAT1_OFFSET]);
+            rsp.q2_offset = decodeProtocolUint16(&buffer[STREAM_RESPONSE_QUAT2_OFFSET]);
+            rsp.q3_offset = decodeProtocolUint16(&buffer[STREAM_RESPONSE_QUAT3_OFFSET]);
+            rsp.q4_offset = decodeProtocolUint16(&buffer[STREAM_RESPONSE_QUAT4_OFFSET]);
+            rsp.flags = decodeProtocolUint16(&buffer[STREAM_RESPONSE_FLAGS]);
+            return STREAM_RESPONSE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int decodeStreamCommand(char *buffer, int length, char &stream_type, unsigned char &update_rate_hz)
+    {
+        if (length < STREAM_CMD_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == '!') && (buffer[1] == MSGID_STREAM_CMD))
+        {
+            if (!verifyChecksum(buffer, STREAM_CMD_CHECKSUM_INDEX))
+                return 0;
+
+            stream_type = buffer[STREAM_CMD_STREAM_TYPE_INDEX];
+            update_rate_hz = decodeUint8(&buffer[STREAM_CMD_UPDATE_RATE_HZ_INDEX]);
+
+            return STREAM_CMD_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int decodeYPRUpdate(char *buffer, int length, struct YPRUpdate &update)
+    {
+        if (length < YPR_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == '!') && (buffer[1] == 'y'))
+        {
+            if (!verifyChecksum(buffer, YPR_UPDATE_CHECKSUM_INDEX))
+                return 0;
+
+            update.yaw = decodeProtocolFloat(&buffer[YPR_UPDATE_YAW_VALUE_INDEX]);
+            update.pitch = decodeProtocolFloat(&buffer[YPR_UPDATE_PITCH_VALUE_INDEX]);
+            update.roll = decodeProtocolFloat(&buffer[YPR_UPDATE_ROLL_VALUE_INDEX]);
+            update.compass_heading = decodeProtocolFloat(&buffer[YPR_UPDATE_COMPASS_VALUE_INDEX]);
+            return YPR_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int decodeQuaternionUpdate(char *buffer, int length, struct QuaternionUpdate &update)
+    {
+        if (length < QUATERNION_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) && (buffer[1] == MSGID_QUATERNION_UPDATE))
+        {
+            if (!verifyChecksum(buffer, QUATERNION_UPDATE_CHECKSUM_INDEX))
+                return 0;
+
+            update.q1 = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_QUAT1_VALUE_INDEX]);
+            update.q2 = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_QUAT2_VALUE_INDEX]);
+            update.q3 = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_QUAT3_VALUE_INDEX]);
+            update.q4 = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_QUAT4_VALUE_INDEX]);
+            update.accel_x = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX]);
+            update.accel_y = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX]);
+            update.accel_z = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX]);
+            update.mag_x = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_MAG_X_VALUE_INDEX]);
+            update.mag_y = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_MAG_Y_VALUE_INDEX]);
+            update.mag_z = (int16_t)decodeProtocolUint16(&buffer[QUATERNION_UPDATE_MAG_Z_VALUE_INDEX]);
+            update.temp_c = decodeProtocolFloat(&buffer[QUATERNION_UPDATE_TEMP_VALUE_INDEX]);
+            return QUATERNION_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+    static int decodeGyroUpdate(char *buffer, int length, struct GyroUpdate &update)
+    {
+        if (length < GYRO_UPDATE_MESSAGE_LENGTH)
+            return 0;
+        if ((buffer[0] == PACKET_START_CHAR) && (buffer[1] == MSGID_GYRO_UPDATE))
+        {
+            if (!verifyChecksum(buffer, GYRO_UPDATE_CHECKSUM_INDEX))
+                return 0;
+
+            update.gyro_x = decodeProtocolUint16(&buffer[GYRO_UPDATE_GYRO_X_VALUE_INDEX]);
+            update.gyro_y = decodeProtocolUint16(&buffer[GYRO_UPDATE_GYRO_Y_VALUE_INDEX]);
+            update.gyro_z = decodeProtocolUint16(&buffer[GYRO_UPDATE_GYRO_Z_VALUE_INDEX]);
+            update.accel_x = decodeProtocolUint16(&buffer[GYRO_UPDATE_ACCEL_X_VALUE_INDEX]);
+            update.accel_y = decodeProtocolUint16(&buffer[GYRO_UPDATE_ACCEL_Y_VALUE_INDEX]);
+            update.accel_z = decodeProtocolUint16(&buffer[GYRO_UPDATE_ACCEL_Z_VALUE_INDEX]);
+            update.mag_x = (int16_t)decodeProtocolUint16(&buffer[GYRO_UPDATE_MAG_X_VALUE_INDEX]);
+            update.mag_y = (int16_t)decodeProtocolUint16(&buffer[GYRO_UPDATE_MAG_Y_VALUE_INDEX]);
+            update.mag_z = (int16_t)decodeProtocolUint16(&buffer[GYRO_UPDATE_MAG_Z_VALUE_INDEX]);
+            update.temp_c = decodeProtocolUnsignedHundredthsFloat(&buffer[GYRO_UPDATE_TEMP_VALUE_INDEX]);
+            return GYRO_UPDATE_MESSAGE_LENGTH;
+        }
+        return 0;
+    }
+
+protected:
+    static void encodeTermination(char *buffer, int total_length, int content_length)
+    {
+        if ((total_length >= (CHECKSUM_LENGTH + TERMINATOR_LENGTH)) && (total_length >= content_length + (CHECKSUM_LENGTH + TERMINATOR_LENGTH)))
+        {
+            // Checksum
+            unsigned char checksum = 0;
+            for (int i = 0; i < content_length; i++)
+            {
+                checksum += buffer[i];
+            }
+            // convert checksum to two ascii bytes
+            sprintf(&buffer[content_length], "%02X", checksum);
+            // Message Terminator
+            sprintf(&buffer[content_length + CHECKSUM_LENGTH], "%s", "\r\n");
+        }
+    }
+
+    // Formats a float as follows
+    //
+    // e.g., -129.235
+    //
+    // "-129.24"
+    //
+    // e.g., 23.4
+    //
+    // "+023.40"
+
+    static void encodeProtocolFloat(float f, char *buff)
+    {
+        char work_buffer[PROTOCOL_FLOAT_LENGTH + 1];
+        int i;
+        int temp1 = abs((int)((f - (int)f) * 100));
+        if (f < 0)
+            buff[0] = '-';
+        else
+            buff[0] = ' ';
+        sprintf(work_buffer, "%03d.%02d", abs((int)f), temp1);
+        for (i = 0; i < (PROTOCOL_FLOAT_LENGTH - 1); i++)
+        {
+            buff[1 + i] = work_buffer[i];
+        }
+    }
+
+    static void encodeProtocolUint16(uint16_t value, char *buff)
+    {
+        sprintf(&buff[0], "%04X", value);
+    }
+
+    static uint16_t decodeProtocolUint16(char *uint16_string)
+    {
+        uint16_t decoded_uint16 = 0;
+        unsigned int shift_left = 12;
+        for (int i = 0; i < 4; i++)
+        {
+            unsigned char digit = uint16_string[i] <= '9' ? uint16_string[i] - '0' : ((uint16_string[i] - 'A') + 10);
+            decoded_uint16 += (((uint16_t)digit) << shift_left);
+            shift_left -= 4;
+        }
+        return decoded_uint16;
+    }
+
+    /* 0 to 655.35 */
+    static inline float decodeProtocolUnsignedHundredthsFloat(char *uint8_unsigned_hundredths_float)
+    {
+        float unsigned_float = (float)decodeProtocolUint16(uint8_unsigned_hundredths_float);
+        unsigned_float /= 100;
+        return unsigned_float;
+    }
+    static inline void encodeProtocolUnsignedHundredthsFloat(float input, char *uint8_unsigned_hundredths_float)
+    {
+        uint16_t input_as_uint = (uint16_t)(input * 100.0f);
+        encodeProtocolUint16(input_as_uint, uint8_unsigned_hundredths_float);
+    }
+
+    static bool verifyChecksum(char *buffer, int content_length)
+    {
+        // Calculate Checksum
+        unsigned char checksum = 0;
+        for (int i = 0; i < content_length; i++)
+        {
+            checksum += buffer[i];
+        }
+
+        // Decode Checksum
+        unsigned char decoded_checksum = decodeUint8(&buffer[content_length]);
+
+        return (checksum == decoded_checksum);
+    }
+
+    static unsigned char decodeUint8(char *checksum)
+    {
+        unsigned char first_digit = checksum[0] <= '9' ? checksum[0] - '0' : ((checksum[0] - 'A') + 10);
+        unsigned char second_digit = checksum[1] <= '9' ? checksum[1] - '0' : ((checksum[1] - 'A') + 10);
+        unsigned char decoded_checksum = (first_digit * 16) + second_digit;
+        return decoded_checksum;
+    }
+
+    static float decodeProtocolFloat(char *buffer)
+    {
+        char temp[PROTOCOL_FLOAT_LENGTH + 1];
+        for (int i = 0; i < PROTOCOL_FLOAT_LENGTH; i++)
+        {
+            temp[i] = buffer[i];
+        }
+        temp[PROTOCOL_FLOAT_LENGTH] = 0;
+        return atof(temp);
+    }
+};
+
+#endif // _IMU_PROTOCOL_H_
diff --git a/src/main/native/include/IMURegisters.h b/src/main/native/include/IMURegisters.h
new file mode 100644
index 0000000..846b7c0
--- /dev/null
+++ b/src/main/native/include/IMURegisters.h
@@ -0,0 +1,496 @@
+/* ============================================
+navX MXP source code is placed under the MIT license
+Copyright (c) 2015 Kauai Labs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+ */
+
+#ifndef IMU_REGISTERS_H_
+#define IMU_REGISTERS_H_
+
+#include "IMUProtocol.h"
+
+/*******************************************************************/
+/*******************************************************************/
+/*                      Register Definitions                       */
+/*******************************************************************/
+/* NOTE:  All multi-byte registers are in little-endian format.    */
+/*        All registers with 'signed' data are twos-complement.    */
+/*        Data Type Summary:                                       */
+/*        unsigned byte:           0   - 255    (8 bits)           */
+/*        unsigned short:          0   - 65535  (16 bits)          */
+/*        signed short:        -32768  - 32767  (16 bits)          */
+/*        signed hundredeths:  -327.68 - 327.67 (16 bits)		   */
+/*        unsigned hundredths:    0.0  - 655.35 (16 bits)          */
+/*        signed thousandths:  -32.768 - 32.767 (16 bits)          */
+/*        signed short ratio: -1/16384 - 1/16384 (16 bits)         */
+/*        16:16:           -32768.9999 - 32767.9999 (32 bits)      */
+/*        unsigned long:             0 - 4294967295 (32 bits)      */
+/*******************************************************************/
+
+typedef int16_t s_short_hundred_float;
+typedef uint16_t u_short_hundred_float;
+typedef int16_t s_short_thousand_float;
+typedef int16_t s_short_ratio_float;
+typedef int32_t s_1616_float;
+
+/**********************************************/
+/* Device Identification Registers            */
+/**********************************************/
+
+#define NAVX_REG_WHOAMI 0x00 /* IMU_MODEL_XXX */
+#define NAVX_REG_HW_REV 0x01
+#define NAVX_REG_FW_VER_MAJOR 0x02
+#define NAVX_REG_FW_VER_MINOR 0x03
+
+/**********************************************/
+/* Status and Control Registers               */
+/**********************************************/
+
+/* Read-write */
+#define NAVX_REG_UPDATE_RATE_HZ 0x04 /* Range:  4 - 200 [unsigned byte] */
+/* Read-only */
+/* Accelerometer Full-Scale Range:  in units of G [unsigned byte] */
+#define NAVX_REG_ACCEL_FSR_G 0x05
+/* Gyro Full-Scale Range (Degrees/Sec):  Range:  250, 500, 1000 or 2000 [unsigned short] */
+#define NAVX_REG_GYRO_FSR_DPS_L 0x06  /* Lower 8-bits of Gyro Full-Scale Range */
+#define NAVX_REG_GYRO_FSR_DPS_H 0x07  /* Upper 8-bits of Gyro Full-Scale Range */
+#define NAVX_REG_OP_STATUS 0x08       /* NAVX_OP_STATUS_XXX */
+#define NAVX_REG_CAL_STATUS 0x09      /* NAVX_CAL_STATUS_XXX */
+#define NAVX_REG_SELFTEST_STATUS 0x0A /* NAVX_SELFTEST_STATUS_XXX */
+#define NAVX_REG_CAPABILITY_FLAGS_L 0x0B
+#define NAVX_REG_CAPABILITY_FLAGS_H 0x0C
+#define NAVX_REG_FW_REVISION_L 0x0D
+#define NAVX_REG_FW_REVISION_H 0x0E
+#define NAVX_REG_FW_PAD_UNUSED2 0x0F
+
+/**********************************************/
+/* Processed Data Registers                   */
+/**********************************************/
+
+#define NAVX_REG_SENSOR_STATUS_L 0x10 /* NAVX_SENSOR_STATUS_XXX */
+#define NAVX_REG_SENSOR_STATUS_H 0x11
+/* Timestamp:  [unsigned long] */
+#define NAVX_REG_TIMESTAMP_L_L 0x12
+#define NAVX_REG_TIMESTAMP_L_H 0x13
+#define NAVX_REG_TIMESTAMP_H_L 0x14
+#define NAVX_REG_TIMESTAMP_H_H 0x15
+
+/* Yaw, Pitch, Roll:  Range: -180.00 to 180.00 [signed hundredths] */
+/* Compass Heading:   Range: 0.00 to 360.00 [unsigned hundredths] */
+/* Altitude in Meters:  In units of meters [16:16] */
+
+#define NAVX_REG_YAW_L 0x16           /* Lower 8 bits of Yaw     */
+#define NAVX_REG_YAW_H 0x17           /* Upper 8 bits of Yaw     */
+#define NAVX_REG_ROLL_L 0x18          /* Lower 8 bits of Roll    */
+#define NAVX_REG_ROLL_H 0x19          /* Upper 8 bits of Roll    */
+#define NAVX_REG_PITCH_L 0x1A         /* Lower 8 bits of Pitch   */
+#define NAVX_REG_PITCH_H 0x1B         /* Upper 8 bits of Pitch   */
+#define NAVX_REG_HEADING_L 0x1C       /* Lower 8 bits of Heading */
+#define NAVX_REG_HEADING_H 0x1D       /* Upper 8 bits of Heading */
+#define NAVX_REG_FUSED_HEADING_L 0x1E /* Upper 8 bits of Fused Heading */
+#define NAVX_REG_FUSED_HEADING_H 0x1F /* Upper 8 bits of Fused Heading */
+#define NAVX_REG_ALTITUDE_I_L 0x20
+#define NAVX_REG_ALTITUDE_I_H 0x21
+#define NAVX_REG_ALTITUDE_D_L 0x22
+#define NAVX_REG_ALTITUDE_D_H 0x23
+
+/* World-frame Linear Acceleration: In units of +/- G * 1000 [signed thousandths] */
+
+#define NAVX_REG_LINEAR_ACC_X_L 0x24 /* Lower 8 bits of Linear Acceleration X */
+#define NAVX_REG_LINEAR_ACC_X_H 0x25 /* Upper 8 bits of Linear Acceleration X */
+#define NAVX_REG_LINEAR_ACC_Y_L 0x26 /* Lower 8 bits of Linear Acceleration Y */
+#define NAVX_REG_LINEAR_ACC_Y_H 0x27 /* Upper 8 bits of Linear Acceleration Y */
+#define NAVX_REG_LINEAR_ACC_Z_L 0x28 /* Lower 8 bits of Linear Acceleration Z */
+#define NAVX_REG_LINEAR_ACC_Z_H 0x29 /* Upper 8 bits of Linear Acceleration Z */
+
+/* Quaternion:  Range -1 to 1 [signed short ratio] */
+
+#define NAVX_REG_QUAT_W_L 0x2A /* Lower 8 bits of Quaternion W */
+#define NAVX_REG_QUAT_W_H 0x2B /* Upper 8 bits of Quaternion W */
+#define NAVX_REG_QUAT_X_L 0x2C /* Lower 8 bits of Quaternion X */
+#define NAVX_REG_QUAT_X_H 0x2D /* Upper 8 bits of Quaternion X */
+#define NAVX_REG_QUAT_Y_L 0x2E /* Lower 8 bits of Quaternion Y */
+#define NAVX_REG_QUAT_Y_H 0x2F /* Upper 8 bits of Quaternion Y */
+#define NAVX_REG_QUAT_Z_L 0x30 /* Lower 8 bits of Quaternion Z */
+#define NAVX_REG_QUAT_Z_H 0x31 /* Upper 8 bits of Quaternion Z */
+
+/**********************************************/
+/* Raw Data Registers                         */
+/**********************************************/
+
+/* Sensor Die Temperature:  Range +/- 150, In units of Centigrade * 100 [signed hundredths float */
+
+#define NAVX_REG_MPU_TEMP_C_L 0x32 /* Lower 8 bits of Temperature */
+#define NAVX_REG_MPU_TEMP_C_H 0x33 /* Upper 8 bits of Temperature */
+
+/* Raw, Calibrated Angular Rotation, in device units.  Value in DPS = units / GYRO_FSR_DPS [signed short] */
+
+#define NAVX_REG_GYRO_X_L 0x34
+#define NAVX_REG_GYRO_X_H 0x35
+#define NAVX_REG_GYRO_Y_L 0x36
+#define NAVX_REG_GYRO_Y_H 0x37
+#define NAVX_REG_GYRO_Z_L 0x38
+#define NAVX_REG_GYRO_Z_H 0x39
+
+/* Raw, Calibrated, Acceleration Data, in device units.  Value in G = units / ACCEL_FSR_G [signed short] */
+
+#define NAVX_REG_ACC_X_L 0x3A
+#define NAVX_REG_ACC_X_H 0x3B
+#define NAVX_REG_ACC_Y_L 0x3C
+#define NAVX_REG_ACC_Y_H 0x3D
+#define NAVX_REG_ACC_Z_L 0x3E
+#define NAVX_REG_ACC_Z_H 0x3F
+
+/* Raw, Calibrated, Un-tilt corrected Magnetometer Data, in device units.  1 unit = 0.15 uTesla [signed short] */
+
+#define NAVX_REG_MAG_X_L 0x40
+#define NAVX_REG_MAG_X_H 0x41
+#define NAVX_REG_MAG_Y_L 0x42
+#define NAVX_REG_MAG_Y_H 0x43
+#define NAVX_REG_MAG_Z_L 0x44
+#define NAVX_REG_MAG_Z_H 0x45
+
+/* Calibrated Pressure in millibars Valid Range:  10.00 Max:  1200.00 [16:16 float]  */
+
+#define NAVX_REG_PRESSURE_IL 0x46
+#define NAVX_REG_PRESSURE_IH 0x47
+#define NAVX_REG_PRESSURE_DL 0x48
+#define NAVX_REG_PRESSURE_DH 0x49
+
+/* Pressure Sensor Die Temperature:  Range +/- 150.00C [signed hundredths] */
+
+#define NAVX_REG_PRESSURE_TEMP_L 0x4A
+#define NAVX_REG_PRESSURE_TEMP_H 0x4B
+
+/**********************************************/
+/* Calibration Registers                      */
+/**********************************************/
+
+/* Yaw Offset: Range -180.00 to 180.00 [signed hundredths] */
+
+#define NAVX_REG_YAW_OFFSET_L 0x4C /* Lower 8 bits of Yaw Offset */
+#define NAVX_REG_YAW_OFFSET_H 0x4D /* Upper 8 bits of Yaw Offset */
+
+/* Hires timestamp:  Range: 0 to 2^64-1 [uint64_t]  */
+
+#define NAVX_REG_HIRES_TIMESTAMP_L_L_L 0x4E /* Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_L_L_H 0x4F /* 2nd Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_L_H_L 0x50 /* 3rd Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_L_H_H 0x51 /* 4th Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_H_L_L 0x52 /* 5th Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_H_L_H 0x53 /* 6th Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_H_H_L 0x54 /* 7th Lowest 8 bits of Hi-res Timestamp */
+#define NAVX_REG_HIRES_TIMESTAMP_H_H_H 0x55 /* Upper 8 bits of Hi-res Timestamp */
+
+/**********************************************/
+/* Integrated Data Registers                  */
+/**********************************************/
+
+/* Integration Control (Write-Only)           */
+#define NAVX_REG_INTEGRATION_CTL 0x56
+#define NAVX_REG_PAD_UNUSED 0x57
+
+/* Velocity:  Range -32768.9999 - 32767.9999 in units of Meters/Sec      */
+
+#define NAVX_REG_VEL_X_I_L 0x58
+#define NAVX_REG_VEL_X_I_H 0x59
+#define NAVX_REG_VEL_X_D_L 0x5A
+#define NAVX_REG_VEL_X_D_H 0x5B
+#define NAVX_REG_VEL_Y_I_L 0x5C
+#define NAVX_REG_VEL_Y_I_H 0x5D
+#define NAVX_REG_VEL_Y_D_L 0x5E
+#define NAVX_REG_VEL_Y_D_H 0x5F
+#define NAVX_REG_VEL_Z_I_L 0x60
+#define NAVX_REG_VEL_Z_I_H 0x61
+#define NAVX_REG_VEL_Z_D_L 0x62
+#define NAVX_REG_VEL_Z_D_H 0x63
+
+/* Displacement:  Range -32768.9999 - 32767.9999 in units of Meters      */
+
+#define NAVX_REG_DISP_X_I_L 0x64
+#define NAVX_REG_DISP_X_I_H 0x65
+#define NAVX_REG_DISP_X_D_L 0x66
+#define NAVX_REG_DISP_X_D_H 0x67
+#define NAVX_REG_DISP_Y_I_L 0x68
+#define NAVX_REG_DISP_Y_I_H 0x69
+#define NAVX_REG_DISP_Y_D_L 0x6A
+#define NAVX_REG_DISP_Y_D_H 0x6B
+#define NAVX_REG_DISP_Z_I_L 0x6C
+#define NAVX_REG_DISP_Z_I_H 0x6D
+#define NAVX_REG_DISP_Z_D_L 0x6E
+#define NAVX_REG_DISP_Z_D_H 0x6F
+
+#define NAVX_REG_LAST NAVX_REG_DISP_Z_D_H
+
+/* NAVX_MODEL */
+
+#define NAVX_MODEL_NAVX_MXP 0x32
+
+/* NAVX_CAL_STATUS */
+
+#define NAVX_CAL_STATUS_IMU_CAL_STATE_MASK 0x03
+#define NAVX_CAL_STATUS_IMU_CAL_INPROGRESS 0x00
+#define NAVX_CAL_STATUS_IMU_CAL_ACCUMULATE 0x01
+#define NAVX_CAL_STATUS_IMU_CAL_COMPLETE 0x02
+
+#define NAVX_CAL_STATUS_MAG_CAL_COMPLETE 0x04
+#define NAVX_CAL_STATUS_BARO_CAL_COMPLETE 0x08
+
+/* NAVX_SELFTEST_STATUS */
+
+#define NAVX_SELFTEST_STATUS_COMPLETE 0x80
+
+#define NAVX_SELFTEST_RESULT_GYRO_PASSED 0x01
+#define NAVX_SELFTEST_RESULT_ACCEL_PASSED 0x02
+#define NAVX_SELFTEST_RESULT_MAG_PASSED 0x04
+#define NAVX_SELFTEST_RESULT_BARO_PASSED 0x08
+
+/* NAVX_OP_STATUS */
+
+#define NAVX_OP_STATUS_INITIALIZING 0x00
+#define NAVX_OP_STATUS_SELFTEST_IN_PROGRESS 0x01
+#define NAVX_OP_STATUS_ERROR 0x02 /* E.g., Self-test fail, I2C error */
+#define NAVX_OP_STATUS_IMU_AUTOCAL_IN_PROGRESS 0x03
+#define NAVX_OP_STATUS_NORMAL 0x04
+
+/* NAVX_SENSOR_STATUS */
+
+#define NAVX_SENSOR_STATUS_MOVING 0x01
+#define NAVX_SENSOR_STATUS_YAW_STABLE 0x02
+#define NAVX_SENSOR_STATUS_MAG_DISTURBANCE 0x04
+#define NAVX_SENSOR_STATUS_ALTITUDE_VALID 0x08
+#define NAVX_SENSOR_STATUS_SEALEVEL_PRESS_SET 0x10
+#define NAVX_SENSOR_STATUS_FUSED_HEADING_VALID 0x20
+
+/* NAVX_REG_CAPABILITY_FLAGS (Aligned w/NAV6 Flags, see IMUProtocol.h) */
+
+#define NAVX_CAPABILITY_FLAG_OMNIMOUNT 0x0004
+#define NAVX_CAPABILITY_FLAG_OMNIMOUNT_CONFIG_MASK 0x0038
+#define NAVX_CAPABILITY_FLAG_VEL_AND_DISP 0x0040
+#define NAVX_CAPABILITY_FLAG_YAW_RESET 0x0080
+#define NAVX_CAPABILITY_FLAG_AHRSPOS_TS 0x0100
+#define NAVX_CAPABILITY_FLAG_FW_REVISION 0x0200
+#define NAVX_CAPABILITY_FLAG_HIRES_TIMESTAMP 0x0400
+#define NAVX_CAPABILITY_FLAG_AHRSPOS_TS_RAW 0x0800
+
+/* NAVX_OMNIMOUNT_CONFIG */
+
+#define OMNIMOUNT_DEFAULT 0 /* Same as Y_Z_UP */
+#define OMNIMOUNT_YAW_X_UP 1
+#define OMNIMOUNT_YAW_X_DOWN 2
+#define OMNIMOUNT_YAW_Y_UP 3
+#define OMNIMOUNT_YAW_Y_DOWN 4
+#define OMNIMOUNT_YAW_Z_UP 5
+#define OMNIMOUNT_YAW_Z_DOWN 6
+
+/* NAVX_INTEGRATION_CTL */
+
+#define NAVX_INTEGRATION_CTL_RESET_VEL_X 0x01
+#define NAVX_INTEGRATION_CTL_RESET_VEL_Y 0x02
+#define NAVX_INTEGRATION_CTL_RESET_VEL_Z 0x04
+#define NAVX_INTEGRATION_CTL_RESET_DISP_X 0x08
+#define NAVX_INTEGRATION_CTL_RESET_DISP_Y 0x10
+#define NAVX_INTEGRATION_CTL_RESET_DISP_Z 0x20
+#define NAVX_INTEGRATION_CTL_VEL_AND_DISP_MASK 0x3F
+
+#define NAVX_INTEGRATION_CTL_RESET_YAW 0x80
+
+class IMURegisters
+{
+public:
+    /************************************************************/
+    /* NOTE:                                                    */
+    /* The following functions assume a little-endian processor */
+    /************************************************************/
+
+    static inline uint16_t decodeProtocolUint16(char *uint16_bytes)
+    {
+        return *((uint16_t *)uint16_bytes);
+    }
+    static inline void encodeProtocolUint16(uint16_t val, char *uint16_bytes)
+    {
+        *((uint16_t *)uint16_bytes) = val;
+    }
+
+    static inline int16_t decodeProtocolInt16(char *int16_bytes)
+    {
+        return *((int16_t *)int16_bytes);
+    }
+    static inline void encodeProtocolInt16(int16_t val, char *int16_bytes)
+    {
+        *((int16_t *)int16_bytes) = val;
+    }
+
+    static inline uint32_t decodeProtocolUint32(char *uint32_bytes)
+    {
+        return *((uint32_t *)uint32_bytes);
+    }
+
+    static inline int32_t decodeProtocolInt32(char *int32_bytes)
+    {
+        return *((int32_t *)int32_bytes);
+    }
+    static inline void encodeProtocolInt32(int32_t val, char *int32_bytes)
+    {
+        *((int32_t *)int32_bytes) = val;
+    }
+
+    /* -327.68 to +327.68 */
+    static inline float decodeProtocolSignedHundredthsFloat(char *uint8_signed_angle_bytes)
+    {
+        float signed_angle = (float)decodeProtocolInt16(uint8_signed_angle_bytes);
+        signed_angle /= 100;
+        return signed_angle;
+    }
+    static inline void encodeProtocolSignedHundredthsFloat(float input, char *uint8_signed_hundredths_float)
+    {
+        int16_t input_as_int = (int16_t)(input * 100.0f);
+        encodeProtocolInt16(input_as_int, uint8_signed_hundredths_float);
+    }
+
+    static inline s_short_hundred_float encodeSignedHundredthsFloat(float input)
+    {
+        return (s_short_hundred_float)(input * 100.0f);
+    }
+    static inline u_short_hundred_float encodeUnsignedHundredthsFloat(float input)
+    {
+        return (u_short_hundred_float)(input * 100.0f);
+    }
+
+    static inline s_short_ratio_float encodeRatioFloat(float input_ratio)
+    {
+        return (s_short_hundred_float)(input_ratio *= 32768.0f);
+    }
+    static inline s_short_thousand_float encodeSignedThousandthsFloat(float input)
+    {
+        return (s_short_thousand_float)(input * 1000.0f);
+    }
+
+    /* 0 to 655.35 */
+    static inline float decodeProtocolUnsignedHundredthsFloat(char *uint8_unsigned_hundredths_float)
+    {
+        float unsigned_float = (float)decodeProtocolUint16(uint8_unsigned_hundredths_float);
+        unsigned_float /= 100;
+        return unsigned_float;
+    }
+    static inline void encodeProtocolUnsignedHundredthsFloat(float input, char *uint8_unsigned_hundredths_float)
+    {
+        uint16_t input_as_uint = (uint16_t)(input * 100.0f);
+        encodeProtocolUint16(input_as_uint, uint8_unsigned_hundredths_float);
+    }
+
+    /* -32.768 to +32.768 */
+    static inline float decodeProtocolSignedThousandthsFloat(char *uint8_signed_angle_bytes)
+    {
+        float signed_angle = (float)decodeProtocolInt16(uint8_signed_angle_bytes);
+        signed_angle /= 1000;
+        return signed_angle;
+    }
+    static inline void encodeProtocolSignedThousandthsFloat(float input, char *uint8_signed_thousandths_float)
+    {
+        int16_t input_as_int = (int16_t)(input * 1000.0f);
+        encodeProtocolInt16(input_as_int, uint8_signed_thousandths_float);
+    }
+
+    /* In units of -1 to 1, multiplied by 16384 */
+    static inline float decodeProtocolRatio(char *uint8_ratio)
+    {
+        float ratio = (float)decodeProtocolInt16(uint8_ratio);
+        ratio /= 32768.0f;
+        return ratio;
+    }
+    static inline void encodeProtocolRatio(float ratio, char *uint8_ratio)
+    {
+        ratio *= 32768.0f;
+        encodeProtocolInt16(ratio, uint8_ratio);
+    }
+
+    /* <int16>.<uint16> (-32768.9999 to 32767.9999) */
+    static float decodeProtocol1616Float(char *uint8_16_16_bytes)
+    {
+        float result = (float)decodeProtocolInt32(uint8_16_16_bytes);
+        result /= 65536.0f;
+        return result;
+    }
+    static void encodeProtocol1616Float(float val, char *uint8_16_16_bytes)
+    {
+        val *= 65536.0f;
+        int32_t packed_float = (int32_t)val;
+        encodeProtocolInt32(packed_float, uint8_16_16_bytes);
+    }
+
+#define CRC7_POLY 0x91
+
+    static void buildCRCLookupTable(uint8_t *table, size_t length)
+    {
+        size_t crc;
+        size_t i, j;
+        if (length == 256)
+        {
+            for (i = 0; i < length; i++)
+            {
+                crc = (uint8_t)i;
+                for (j = 0; j < 8; j++)
+                {
+                    if (crc & 1)
+                    {
+                        crc ^= CRC7_POLY;
+                    }
+                    crc >>= 1;
+                }
+                table[i] = crc;
+            }
+        }
+    }
+
+    static inline uint8_t getCRCWithTable(uint8_t *table, uint8_t message[], uint8_t length)
+    {
+        uint8_t i, crc = 0;
+
+        for (i = 0; i < length; i++)
+        {
+            crc ^= message[i];
+            crc = table[crc];
+        }
+        return crc;
+    }
+
+    static uint8_t getCRC(uint8_t message[], uint8_t length)
+    {
+        uint8_t i, j, crc = 0;
+
+        for (i = 0; i < length; i++)
+        {
+            crc ^= message[i];
+            for (j = 0; j < 8; j++)
+            {
+                if (crc & 1)
+                {
+                    crc ^= CRC7_POLY;
+                }
+                crc >>= 1;
+            }
+        }
+        return crc;
+    }
+};
+
+#endif /* IMU_REGISTERS_H_ */
diff --git a/src/main/native/include/ITimestampedDataSubscriber.h b/src/main/native/include/ITimestampedDataSubscriber.h
new file mode 100644
index 0000000..45b880f
--- /dev/null
+++ b/src/main/native/include/ITimestampedDataSubscriber.h
@@ -0,0 +1,30 @@
+#include "AHRSProtocol.h"
+
+/**
+ * The ITimestampedDataSubscriber interface provides a method for consumers
+ * of navX-Model device data to be rapidly notified whenever new data
+ * has arrived.
+ *
+ * - timestampedDataReceived():  reception of sensor-timestamped data
+ *
+ *  A "sensor" timestamp is provided, generated by the navX-Model device, which is
+ *  at millisecond resolution.  A "system timestamp", also at millisecond resolution,
+ *  is also provided, which represents as accurately as possible the time at which the
+ *  data was acquired from the navX-Model device.  Note that the "system timestamp"
+ *  typically has more jitter since it is generated by the host of the navX-Model
+ *  device.
+ *
+ *  Thus, in general sensor timestamps are preferred, as they are generated
+ *  by the navX-Model device motion processor and has a greater accuracy (+/- 1ms) than the
+ *  system timestamp which is vulnerable to latencies introduced by the
+ *  host operating system.
+ *
+ *  The system timestamp is provided to allow performance monitoring of the
+ *  navX-Model device host's data acquisition process.
+ **/
+class ITimestampedDataSubscriber
+{
+public:
+    virtual ~ITimestampedDataSubscriber() {}
+    virtual void timestampedDataReceived(long system_timestamp, long sensor_timestamp, AHRSProtocol::AHRSUpdateBase &sensor_data, void *context) = 0;
+};
diff --git a/src/main/native/include/header.h b/src/main/native/include/header.h
deleted file mode 100644
index e69de29..0000000