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