diff --git a/CMakeLists.txt b/CMakeLists.txt
index b8d4c461a938bb420398e9cd5fd304643287774a..865504715433f0b8567ff652b77d4b5652f54bed 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -156,7 +156,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/src/calibration.cpp b/src/calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1be3341dc844b663da2351ca348c2e341c2daee
--- /dev/null
+++ b/src/calibration.cpp
@@ -0,0 +1,59 @@
+#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++);
+ }
+ }
+}
+
+// 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/src/calibration.h b/src/calibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..357e02cddde8611d51ed87c8b528dd429f23e66a
--- /dev/null
+++ b/src/calibration.h
@@ -0,0 +1,43 @@
+/// \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();
+
+ /// \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);
+
+ // 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/ros_node.cpp b/src/ros_node.cpp
index 4aa670a9699cffae70f36f38737e64c236e599f3..9b2431e96cbfec24ab07412c21a155d48a77e145 100644
--- a/src/ros_node.cpp
+++ b/src/ros_node.cpp
@@ -29,6 +29,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);
@@ -97,6 +100,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 +111,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 +125,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/src/ros_node.h b/src/ros_node.h
index 82cb1fd5fc109c6f7b899f9dc90bc9f8e7be101e..841e90395ac7eca3a5b7cbfa70439feb3b474fca 100644
--- a/src/ros_node.h
+++ b/src/ros_node.h
@@ -4,8 +4,10 @@
#define ROS_NODE_H
#include "driver.h"
+#include "calibration.h"
#include <ros/ros.h>
+#include <std_srvs/Trigger.h>
/// \brief Implements the driver's ROS node functionality.
class ros_node
@@ -24,11 +26,23 @@ public:
void spin();
private:
- // VARIABLES
+ // 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;
+
+ // 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.