From 2f2ca7942f3f4ea96d37e0de1791cca4b256cca6 Mon Sep 17 00:00:00 2001
From: Paul D'Angio <pcdangio@gmail.com>
Date: Mon, 26 Oct 2020 21:50:01 -0400
Subject: [PATCH] added gyroscope calibration service

---
 driver_mpu9250/src/ros_node.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/driver_mpu9250/src/ros_node.cpp b/driver_mpu9250/src/ros_node.cpp
index 2c26b76..5cc76d8 100644
--- a/driver_mpu9250/src/ros_node.cpp
+++ b/driver_mpu9250/src/ros_node.cpp
@@ -67,6 +67,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
         ros::shutdown();
     }
 
+    // Set up services.
+    ros_node::m_service_calibrate_gyroscope = ros_node::m_node->advertiseService("imu/calibrate_gyroscope", &ros_node::service_calibrate_gyroscope, this);
+
     // Perform initial gyroscope calibration.
     ros_node::calibrate_gyroscope(500);
 }
@@ -182,6 +185,11 @@ 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;
+    // If gyroscope calibration is running, add uncalibrate data to window.
+    if(ros_node::f_gyroscope_calibrating)
+    {
+        ros_node::m_gyroscope_calibration_window.push_back({message_gyro.x, message_gyro.y, message_gyro.z});
+    }
     // Apply calibration.
     ros_node::m_calibration_gyroscope.calibrate(message_gyro.x, message_gyro.y, message_gyro.z);
     // Publish message.
-- 
GitLab