From 508957b901d4c8b57e287a90cc8e5950d64506ae Mon Sep 17 00:00:00 2001
From: Paul D'Angio <pcdangio@gmail.com>
Date: Mon, 26 Oct 2020 21:16:06 -0400
Subject: [PATCH] added gyroscope calibration service and routine
---
driver_mpu9250/CMakeLists.txt | 1 +
driver_mpu9250/package.xml | 1 +
driver_mpu9250/src/calibration.cpp | 4 ++
driver_mpu9250/src/calibration.h | 4 ++
driver_mpu9250/src/ros_node.cpp | 67 ++++++++++++++++++++++++++++++
driver_mpu9250/src/ros_node.h | 23 +++++++++-
6 files changed, 99 insertions(+), 1 deletion(-)
diff --git a/driver_mpu9250/CMakeLists.txt b/driver_mpu9250/CMakeLists.txt
index 8655047..f83966c 100644
--- a/driver_mpu9250/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
)
diff --git a/driver_mpu9250/package.xml b/driver_mpu9250/package.xml
index aabfc13..f7b34db 100644
--- a/driver_mpu9250/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
index e1be334..2ef0c54 100644
--- a/driver_mpu9250/src/calibration.cpp
+++ b/driver_mpu9250/src/calibration.cpp
@@ -40,6 +40,10 @@ void calibration::load(ros::NodeHandle& node_handle, std::string param_name)
}
}
}
+void calibration::update(const Eigen::Matrix4d& new_transform)
+{
+ calibration::m_calibration = new_transform;
+}
// CALIBRATION
void calibration::calibrate(double& x, double& y, double& z)
diff --git a/driver_mpu9250/src/calibration.h b/driver_mpu9250/src/calibration.h
index 357e02c..9e54531 100644
--- a/driver_mpu9250/src/calibration.h
+++ b/driver_mpu9250/src/calibration.h
@@ -15,11 +15,15 @@ public:
/// \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.
diff --git a/driver_mpu9250/src/ros_node.cpp b/driver_mpu9250/src/ros_node.cpp
index d7f9213..2c26b76 100644
--- a/driver_mpu9250/src/ros_node.cpp
+++ b/driver_mpu9250/src/ros_node.cpp
@@ -10,6 +10,9 @@
// 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;
@@ -63,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()
{
@@ -81,6 +87,67 @@ 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()
{
diff --git a/driver_mpu9250/src/ros_node.h b/driver_mpu9250/src/ros_node.h
index f533bd9..b4e93df 100644
--- a/driver_mpu9250/src/ros_node.h
+++ b/driver_mpu9250/src/ros_node.h
@@ -7,7 +7,9 @@
#include "calibration.h"
#include <ros/ros.h>
-#include <std_srvs/Trigger.h>
+#include <driver_mpu9250_msgs/calibrate_gyroscope.h>
+
+#include <deque>
/// \brief Implements the driver's ROS node functionality.
class ros_node
@@ -38,6 +40,16 @@ private:
/// \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;
@@ -52,6 +64,15 @@ private:
/// \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();
--
GitLab