diff --git a/CMakeLists.txt b/driver_mpu9250/CMakeLists.txt
similarity index 98%
rename from CMakeLists.txt
rename to driver_mpu9250/CMakeLists.txt
index b8d4c461a938bb420398e9cd5fd304643287774a..f83966cdeb6efabdbc6f3ae7d1407bdb1e75db8f 100644
--- a/CMakeLists.txt
+++ b/driver_mpu9250/CMakeLists.txt
@@ -9,6 +9,7 @@ add_compile_options(-std=c++11)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
+  driver_mpu9250_msgs
   sensor_msgs_ext
 )
 
@@ -156,7 +157,7 @@ if(pigpiod)
     ## Declare a C++ executable
     ## With catkin_make all packages are built within a single CMake context
     ## The recommended prefix ensures that target names across packages don't collide
-    add_executable(${PROJECT_NAME}_rpi_node src/main_rpi.cpp src/ros_node.cpp src/rpi_driver.cpp src/driver.cpp)
+    add_executable(${PROJECT_NAME}_rpi_node src/main_rpi.cpp src/ros_node.cpp src/calibration.cpp src/rpi_driver.cpp src/driver.cpp)
     ## Rename C++ executable without prefix
     ## The above recommended prefix causes long target names, the following renames the
     ## target back to the shorter version for ease of user use
diff --git a/doxyfile b/driver_mpu9250/doxyfile
similarity index 100%
rename from doxyfile
rename to driver_mpu9250/doxyfile
diff --git a/package.xml b/driver_mpu9250/package.xml
similarity index 92%
rename from package.xml
rename to driver_mpu9250/package.xml
index aabfc13576378792593fc83b5f137b5e96e12ac3..f7b34db8c15a2e8d643409cfac081f2b953904b1 100644
--- a/package.xml
+++ b/driver_mpu9250/package.xml
@@ -10,6 +10,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
+  <depend>driver_mpu9250_msgs</depend>
   <depend>sensor_msgs_ext</depend>
 
 </package>
diff --git a/driver_mpu9250/src/calibration.cpp b/driver_mpu9250/src/calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ef0c549515c405a587b704085610583454f26dc
--- /dev/null
+++ b/driver_mpu9250/src/calibration.cpp
@@ -0,0 +1,63 @@
+#include "calibration.h"
+
+#include <ros/names.h>
+
+// CONSTRUCTORS
+calibration::calibration()
+{
+    // Initialize matrices/vectors.
+    calibration::m_calibration.setIdentity();
+    calibration::m_u.setZero();
+    calibration::m_u(3) = 1.0;
+    calibration::m_c.setZero();
+}
+
+// CONFIGURATION
+void calibration::load(ros::NodeHandle& node_handle, std::string param_name)
+{
+    // Try reading the calibration parameter.
+    std::vector<double> components;
+    if(!node_handle.getParam(param_name, components))
+    {
+        // Param not found, quit.
+        return;
+    }
+
+    // Check validity of parameter.
+    if(components.size() != 16)
+    {
+        ROS_ERROR_STREAM("invalid parameter for 4x4 calibration matrix: " << param_name);
+        return;
+    }
+
+    // Read in row-step order.
+    uint32_t k = 0;
+    for(uint32_t i = 0; i < calibration::m_calibration.rows(); ++i)
+    {
+        for(uint32_t j = 0; j < calibration::m_calibration.cols(); ++j)
+        {
+            calibration::m_calibration(i,j) = components.at(k++);
+        }
+    }
+}
+void calibration::update(const Eigen::Matrix4d& new_transform)
+{
+    calibration::m_calibration = new_transform;
+}
+
+// CALIBRATION
+void calibration::calibrate(double& x, double& y, double& z)
+{
+    // Store point into vector.
+    calibration::m_u(0) = x;
+    calibration::m_u(1) = y;
+    calibration::m_u(2) = z;
+
+    // Calibrate point.
+    calibration::m_c.noalias() = calibration::m_calibration * calibration::m_u;
+
+    // Output calibrated point.
+    x = calibration::m_c(0);
+    y = calibration::m_c(1);
+    z = calibration::m_c(2);
+}
\ No newline at end of file
diff --git a/driver_mpu9250/src/calibration.h b/driver_mpu9250/src/calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e54531538503f8350f5a09ab5e9ec146031d8d3
--- /dev/null
+++ b/driver_mpu9250/src/calibration.h
@@ -0,0 +1,47 @@
+/// \file calibration.h
+/// \brief Defines the calibration class.
+#ifndef MPU9250_CALIBRATION_H
+#define MPU9250_CALIBRATION_H
+
+#include <ros/node_handle.h>
+#include <vector>
+#include <eigen3/Eigen/Dense>
+
+/// \brief Performs calibration on 3D points.
+class calibration
+{
+public:
+    // CONSTRUCTORS
+    /// \brief Creates a new calibration instance with a unity calibration matrix.
+    calibration();
+
+    // CONFIGURATION
+    /// \brief Loads a calibration matrix from a ROS parameter.
+    /// \param node_handle The node handle to read the parameter from.
+    /// \param param_name The name of the ROS parameter to read from.
+    /// \note If the parameter doesn't exist or is invalid, the calibration remains unchanged.
+    void load(ros::NodeHandle& node_handle, std::string param_name);
+    /// \brief Updates the calibration's transformation matrix.
+    /// \param new_calibration The new transformation matrix to set.
+    void update(const Eigen::Matrix4d& new_transform);
+
+    // CALIBRATION
+    /// \brief Performs an in-place calibration on a 3D point.
+    /// \param x The x component to calibrate.
+    /// \param y The y component to calibrate.
+    /// \param z The z component to calibrate.
+    void calibrate(double& x, double& y, double& z);
+
+private:
+    // CALIBRATION
+    /// \brief Stores the calibration as a homogeneous transformation matrix.
+    Eigen::Matrix4d m_calibration;
+
+    // PREALLOCATIONS
+    /// \brief An uncalibrated point.
+    Eigen::Vector4d m_u;
+    /// \brief A calibrated point.
+    Eigen::Vector4d m_c;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/driver.cpp b/driver_mpu9250/src/driver.cpp
similarity index 100%
rename from src/driver.cpp
rename to driver_mpu9250/src/driver.cpp
diff --git a/src/driver.h b/driver_mpu9250/src/driver.h
similarity index 99%
rename from src/driver.h
rename to driver_mpu9250/src/driver.h
index 8b28595348183a0d066381195bc29d67932b3459..afe15483f4dc30158b57ac6cd89f7eecfbcf7eb7 100644
--- a/src/driver.h
+++ b/driver_mpu9250/src/driver.h
@@ -1,7 +1,7 @@
 /// \file driver.h
 /// \brief Defines the MPU9250 driver class.
-#ifndef DRIVER_H
-#define DRIVER_H
+#ifndef MPU9250_DRIVER_H
+#define MPU9250_DRIVER_H
 
 #include <functional>
 
diff --git a/src/main_rpi.cpp b/driver_mpu9250/src/main_rpi.cpp
similarity index 100%
rename from src/main_rpi.cpp
rename to driver_mpu9250/src/main_rpi.cpp
diff --git a/src/ros_node.cpp b/driver_mpu9250/src/ros_node.cpp
similarity index 64%
rename from src/ros_node.cpp
rename to driver_mpu9250/src/ros_node.cpp
index 4aa670a9699cffae70f36f38737e64c236e599f3..2c26b76d749ef3458b46f26a2836fe85be87f949 100644
--- a/src/ros_node.cpp
+++ b/driver_mpu9250/src/ros_node.cpp
@@ -7,8 +7,12 @@
 
 #include <cmath>
 
+// CONSTRUCTORS
 ros_node::ros_node(driver *driver, int argc, char **argv)
 {
+    // Initialize flags.
+    ros_node::f_gyroscope_calibrating = false;
+
     // Create a new driver.
     ros_node::m_driver = driver;
 
@@ -29,6 +33,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
     int param_accel_fsr = private_node.param<int>("accel_fsr", 0);
     float param_max_data_rate = private_node.param<float>("max_data_rate", 8000.0F);
 
+    // Read calibrations.
+    ros_node::m_calibration_accelerometer.load(private_node, "calibration_accelerometer");
+    ros_node::m_calibration_magnetometer.load(private_node, "calibration_magnetometer");
 
     // Set up publishers.
     ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::acceleration>("imu/accelerometer", 1);
@@ -59,6 +66,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
         // Quit the node.
         ros::shutdown();
     }
+
+    // Perform initial gyroscope calibration.
+    ros_node::calibrate_gyroscope(500);
 }
 ros_node::~ros_node()
 {
@@ -67,6 +77,7 @@ ros_node::~ros_node()
     delete ros_node::m_driver;
 }
 
+// ROS
 void ros_node::spin()
 {
     // Spin.
@@ -76,6 +87,68 @@ void ros_node::spin()
     ros_node::deinitialize_driver();
 }
 
+// SERVICES
+bool ros_node::service_calibrate_gyroscope(driver_mpu9250_msgs::calibrate_gyroscopeRequest& request, driver_mpu9250_msgs::calibrate_gyroscopeResponse& response)
+{
+    response.success = ros_node::calibrate_gyroscope(request.averaging_period);
+    if(!response.success)
+    {
+        response.message = "failed to collect enough points for calibration";
+    }
+
+    return true;
+}
+bool ros_node::calibrate_gyroscope(uint32_t averaging_period)
+{
+    // Convert averaging period to duration.
+    ros::Duration averaging_duration(static_cast<double>(averaging_period) / 1000.0);
+
+    // Clear the collection window.
+    ros_node::m_gyroscope_calibration_window.clear();
+
+    // Enable the data collection.
+    ros_node::f_gyroscope_calibrating = true;
+
+    // Sleep while gyro data is collected on interrupt thread.
+    averaging_duration.sleep();
+
+    // Disable the data collection.
+    ros_node::f_gyroscope_calibrating = false;
+
+    // Check if the window contains data.
+    if(ros_node::m_gyroscope_calibration_window.size() < 5)
+    {
+        ROS_ERROR_STREAM("gyroscope calibration failed (not enough data: " << ros_node::m_gyroscope_calibration_window.size() << " points)");
+        return false;
+    }
+
+    // Iterate through window to calculate average.
+    Eigen::Vector3d average;
+    average.setZero();
+    for(auto point = ros_node::m_gyroscope_calibration_window.cbegin(); point != ros_node::m_gyroscope_calibration_window.cend(); ++point)
+    {
+        average += *point;
+    }
+    average /= static_cast<double>(ros_node::m_gyroscope_calibration_window.size());
+
+    // Clear window.
+    ros_node::m_gyroscope_calibration_window.clear();
+
+    // Create new homogeneous transform by subtracting out bias.
+    Eigen::Matrix4d calibration;
+    calibration.setIdentity();
+    calibration.block(0, 3, 3, 1) = -average;
+
+    // Update gyroscope calibration.
+    ros_node::m_calibration_gyroscope.update(calibration);
+
+    // Log success.
+    ROS_INFO_STREAM("gyroscope calibration completed with averaging period of " << averaging_period << " ms");
+
+    return true;
+}
+
+// METHODS
 void ros_node::deinitialize_driver()
 {
     try
@@ -89,6 +162,7 @@ void ros_node::deinitialize_driver()
     }
 }
 
+// CALLBACKS
 void ros_node::data_callback(driver::data data)
 {
     // Create accelerometer message.
@@ -97,6 +171,8 @@ void ros_node::data_callback(driver::data data)
     message_accel.x = static_cast<double>(data.accel_x) * 9.80665;
     message_accel.y = static_cast<double>(data.accel_y) * 9.80665;
     message_accel.z = static_cast<double>(data.accel_z) * 9.80665;
+    // Apply calibration.
+    ros_node::m_calibration_accelerometer.calibrate(message_accel.x, message_accel.y, message_accel.z);
     // Publish message.
     ros_node::m_publisher_accelerometer.publish(message_accel);
 
@@ -106,6 +182,8 @@ void ros_node::data_callback(driver::data data)
     message_gyro.x = static_cast<double>(data.gyro_x) * M_PI / 180.0;
     message_gyro.y = static_cast<double>(data.gyro_y) * M_PI / 180.0;
     message_gyro.z = static_cast<double>(data.gyro_z) * M_PI / 180.0;
+    // Apply calibration.
+    ros_node::m_calibration_gyroscope.calibrate(message_gyro.x, message_gyro.y, message_gyro.z);
     // Publish message.
     ros_node::m_publisher_gyroscope.publish(message_gyro);
 
@@ -118,6 +196,8 @@ void ros_node::data_callback(driver::data data)
         message_mag.x = static_cast<double>(data.magneto_x) * 0.000001;
         message_mag.y = static_cast<double>(data.magneto_y) * 0.000001;
         message_mag.z = static_cast<double>(data.magneto_z) * 0.000001;
+        // Apply calibration.
+        ros_node::m_calibration_magnetometer.calibrate(message_mag.x, message_mag.y, message_mag.z);
         // Publish message.
         ros_node::m_publisher_magnetometer.publish(message_mag);
     }
diff --git a/driver_mpu9250/src/ros_node.h b/driver_mpu9250/src/ros_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4e93df3567eaa711db8183d8ff2182827f6c35d
--- /dev/null
+++ b/driver_mpu9250/src/ros_node.h
@@ -0,0 +1,86 @@
+/// \file ros_node.h
+/// \brief Defines the ros_node class.
+#ifndef ROS_NODE_H
+#define ROS_NODE_H
+
+#include "driver.h"
+#include "calibration.h"
+
+#include <ros/ros.h>
+#include <driver_mpu9250_msgs/calibrate_gyroscope.h>
+
+#include <deque>
+
+/// \brief Implements the driver's ROS node functionality.
+class ros_node
+{
+public:
+    // CONSTRUCTORS
+    /// \brief ros_node Initializes the ROS node.
+    /// \param driver The MPU9250 driver instance.
+    /// \param argc Number of main() args.
+    /// \param argv The main() args.
+    ros_node(driver* driver, int argc, char **argv);
+    ~ros_node();
+
+    // METHODS
+    /// \brief spin Runs the node.
+    void spin();
+
+private:
+    // COMPONENTS
+    /// \brief m_driver The driver instance.
+    driver* m_driver;
+
+    // CALIBRATIONS
+    /// \brief The accelerometer's calibration.
+    calibration m_calibration_accelerometer;
+    /// \brief The gyroscope's calibration.
+    calibration m_calibration_gyroscope;
+    /// \brief The magnetometer's calibration.
+    calibration m_calibration_magnetometer;
+
+    // GYROSCOPE CALIBRATION
+    /// \brief The calibration data window for the gyroscope.
+    std::deque<Eigen::Vector3d> m_gyroscope_calibration_window;
+    /// \brief Flag indiciating if the gyroscope is currently calibrating.
+    std::atomic<bool> f_gyroscope_calibrating;
+    /// \brief Runs a zero-velocity calibration on the gyroscope to remove bias.
+    /// \param averaging_period The number of milliseconds to average data over to calculate bias.
+    /// \returns TRUE if the calibration succeeded, otherwise FALSE.
+    bool calibrate_gyroscope(uint32_t averaging_period);
+
+    // ROS
+    /// \brief m_node The node's handle.
+    ros::NodeHandle* m_node;
+
+    // PUBLISHERS
+    /// \brief Publisher for accelerometer data.
+    ros::Publisher m_publisher_accelerometer;
+    /// \brief Publisher for gyroscope data.
+    ros::Publisher m_publisher_gyroscope;
+    /// \brief Publisher for magnetometer data.
+    ros::Publisher m_publisher_magnetometer;
+    /// \brief Publisher for temperature data.
+    ros::Publisher m_publisher_temperature;
+
+    // SERVICES
+    /// \brief Service server for calibrating the gyroscope.
+    ros::ServiceServer m_service_calibrate_gyroscope;
+    /// \brief A service for calibrating the gyroscope.
+    /// \param request The service request.
+    /// \param response The service response.
+    /// \returns TRUE if the service completed successfully, otherwise FALSE.
+    bool service_calibrate_gyroscope(driver_mpu9250_msgs::calibrate_gyroscopeRequest& request, driver_mpu9250_msgs::calibrate_gyroscopeResponse& response);
+
+    // METHODS
+    /// \brief deinitialize_driver Deinitializes the driver.
+    void deinitialize_driver();
+
+    // CALLBACKS
+    /// \brief data_callback The callback function for when new data is available.
+    /// \param data The latest data read from the MPU9250/AK8963.
+    void data_callback(driver::data data);
+};
+
+#endif // ROS_NODE_H
diff --git a/src/rpi_driver.cpp b/driver_mpu9250/src/rpi_driver.cpp
similarity index 100%
rename from src/rpi_driver.cpp
rename to driver_mpu9250/src/rpi_driver.cpp
diff --git a/src/rpi_driver.h b/driver_mpu9250/src/rpi_driver.h
similarity index 100%
rename from src/rpi_driver.h
rename to driver_mpu9250/src/rpi_driver.h
diff --git a/driver_mpu9250_msgs/CMakeLists.txt b/driver_mpu9250_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..04b0dc46945007b9c3e57ed7f9f1f534def44954
--- /dev/null
+++ b/driver_mpu9250_msgs/CMakeLists.txt
@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(driver_mpu9250_msgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+    message_generation)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+# Generate services in the 'srv' folder
+add_service_files(
+    FILES
+    calibrate_gyroscope.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES driver_mpu9250_msgs
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
\ No newline at end of file
diff --git a/driver_mpu9250_msgs/package.xml b/driver_mpu9250_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ac64dbc754d75bd8d5884ecf03baa55e7ecf747f
--- /dev/null
+++ b/driver_mpu9250_msgs/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+
+  <name>driver_mpu9250_msgs</name>
+  <version>1.0.0</version>
+  <description>Provides messages and services for interacting with the driver_mpu9250 node.</description>
+
+  <author email="pcdangio@gmail.com">Paul D'Angio</author>
+  <maintainer email="pcdangio@gmail.com">Paul D'Angio</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+
+</package>
\ No newline at end of file
diff --git a/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv b/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv
new file mode 100644
index 0000000000000000000000000000000000000000..f04afd6035b18f5e6e7ded5333b1d19b3240d449
--- /dev/null
+++ b/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv
@@ -0,0 +1,6 @@
+# Requests for the IMU to perform a zero-velocity calibration of the gyroscope.
+
+uint32 averaging_period     # The number of milliseconds to average the bias calculation over.
+---
+bool success                # TRUE if the calibration was successful, otherwise FALSE.
+string message              # If unsuccesful, reports the reason.
\ No newline at end of file
diff --git a/src/ros_node.h b/src/ros_node.h
deleted file mode 100644
index 82cb1fd5fc109c6f7b899f9dc90bc9f8e7be101e..0000000000000000000000000000000000000000
--- a/src/ros_node.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/// \file ros_node.h
-/// \brief Defines the ros_node class.
-#ifndef ROS_NODE_H
-#define ROS_NODE_H
-
-#include "driver.h"
-
-#include <ros/ros.h>
-
-/// \brief Implements the driver's ROS node functionality.
-class ros_node
-{
-public:
-    // CONSTRUCTORS
-    /// \brief ros_node Initializes the ROS node.
-    /// \param driver The MPU9250 driver instance.
-    /// \param argc Number of main() args.
-    /// \param argv The main() args.
-    ros_node(driver* driver, int argc, char **argv);
-    ~ros_node();
-
-    // METHODS
-    /// \brief spin Runs the node.
-    void spin();
-
-private:
-    // VARIABLES
-    /// \brief m_driver The driver instance.
-    driver* m_driver;
-    /// \brief m_node The node's handle.
-    ros::NodeHandle* m_node;
-    /// \brief Publisher for accelerometer data.
-    ros::Publisher m_publisher_accelerometer;
-    /// \brief Publisher for gyroscope data.
-    ros::Publisher m_publisher_gyroscope;
-    /// \brief Publisher for magnetometer data.
-    ros::Publisher m_publisher_magnetometer;
-    /// \brief Publisher for temperature data.
-    ros::Publisher m_publisher_temperature;
-
-    // METHODS
-    /// \brief deinitialize_driver Deinitializes the driver.
-    void deinitialize_driver();
-
-    /// \brief data_callback The callback function for when new data is available.
-    /// \param data The latest data read from the MPU9250/AK8963.
-    void data_callback(driver::data data);
-};
-
-#endif // ROS_NODE_H