diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f17c1f..2f8d3ea 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ set(CATKIN_DEPS
find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPS})
find_package(Eigen3 REQUIRED)
find_package(cmake_modules REQUIRED)
+find_package(yaml-cpp REQUIRED)
catkin_package(
INCLUDE_DIRS include
@@ -52,6 +53,31 @@ target_link_libraries(mpu6050_calibration_node
${MPU6050_NODE_LIB}
)
+add_executable(mpu6050_calibration_node2
+ src/mpu6050_calibration_node2.cpp
+)
+
+target_link_libraries(mpu6050_calibration_node2
+ ${catkin_LIBRARIES}
+ ${MPU6050_NODE_LIB}
+ yaml-cpp
+)
+
+add_executable(mpu6050_node2
+ src/mpu6050_node2.cpp
+)
+
+target_link_libraries(mpu6050_node2
+ ${catkin_LIBRARIES}
+ ${MPU6050_NODE_LIB}
+)
+
+
+# Install Python scripts
+catkin_install_python(PROGRAMS scripts/imu_calibration_listener.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
diff --git a/config/mpu_settings.yaml b/config/mpu_settings.yaml
index 12a4517..23bbab6 100644
--- a/config/mpu_settings.yaml
+++ b/config/mpu_settings.yaml
@@ -1,23 +1,23 @@
-# I2C Bus URI used to comunicate with I2C devices (default: "/dev/i2c-1")
-bus_uri: "/dev/i2c-1"
-
-# I2C address of MPU6050 (default: 0x68)
+bus_uri: /dev/i2c-1
mpu_address: 0x68
-
-# Frequency in Hertz wich IMU data is published (default: 30)
-pub_rate: 25
-
-# Frame if of IMU message (default: "imu")
-frame_id: "imu"
-
-# Offsets to fix wrong values caused by misalignment
-# Sequence is (ax, ay, az, gx, gy, gz) (default: [0, 0, 0, 0, 0, 0])
-axes_offsets: [-1987, 44, 1210, 75, -10, 7]
-
-# PID constants used in calibration procedure
-ki: 0.2 # (default: 0.1)
-kp: 0.1 # (default: 0.1)
-
-# The calibration process is finished when the error is aproximate zero with
-# the precision set by delta (default: 0.5)
-delta: 0.5
+pub_rate: 200
+frame_id: imu
+axes_offsets: [0, 0, 0, 0, 0, 0]
+ki: 0.20000000000000001
+kp: 0.10000000000000001
+delta: 0.20000000000000001
+Xgain: 0.99591491699218748
+YtoX: -0.0031604003906250001
+ZtoX: 0.081433105468750003
+Xofs: -0.00078877766927083336
+XtoY: -0.0031121826171875001
+Ygain: 0.99959594726562506
+ZtoY: 0.0064855957031250003
+Yofs: -0.0081247965494791671
+XtoZ: -0.098767089843750003
+YtoZ: -0.032595825195312497
+Zgain: 1.0119696044921875
+Zofs: -0.00021769205729166668
+gyro_x_offset: -2.460000010728836
+gyro_y_offset: 8.7046183967590327
+gyro_z_offset: 1.5921755743026733
\ No newline at end of file
diff --git a/launch/imu_calibration_listener.launch b/launch/imu_calibration_listener.launch
new file mode 100644
index 0000000..1a31029
--- /dev/null
+++ b/launch/imu_calibration_listener.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/mpu6050_calibration2.launch b/launch/mpu6050_calibration2.launch
new file mode 100644
index 0000000..68012e9
--- /dev/null
+++ b/launch/mpu6050_calibration2.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/mpu6050_driver.launch b/launch/mpu6050_driver.launch
index 57af565..ce6da0e 100644
--- a/launch/mpu6050_driver.launch
+++ b/launch/mpu6050_driver.launch
@@ -1,8 +1,9 @@
-
-
+
+
+
diff --git a/launch/mpu6050_driver2.launch b/launch/mpu6050_driver2.launch
new file mode 100644
index 0000000..c7b2d36
--- /dev/null
+++ b/launch/mpu6050_driver2.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
diff --git a/scripts/imu_calibration_listener.py b/scripts/imu_calibration_listener.py
new file mode 100755
index 0000000..4cbfca6
--- /dev/null
+++ b/scripts/imu_calibration_listener.py
@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import Imu
+import yaml
+import os
+import time
+
+class FlowStyleListDumper(yaml.Dumper):
+ def increase_indent(self, flow=False, indentless=False):
+ return super(FlowStyleListDumper, self).increase_indent(flow=True)
+
+class IMUCalibrationListener:
+ def __init__(self):
+ self.node_name = 'mpu_calibration_listener'
+ rospy.init_node(self.node_name, anonymous=True)
+
+ self.yaml_path = rospy.get_param('~mpu_settings_path', '')
+ if not self.yaml_path:
+ rospy.logerr("YAML file path must be specified as a private parameter.")
+ exit(1)
+
+ self.last_time_received = time.time()
+ self.timeout_duration = 5 # Time in seconds to wait after the last message
+ self.last_offsets = None
+
+ rospy.Subscriber('/imu_offsets', Imu, self.imu_callback)
+
+ rospy.loginfo(f"{self.node_name} started.")
+ rospy.loginfo("Please ensure the IMU is placed level on a flat surface and remains motionless during calibration.")
+ rospy.loginfo(f"Loading configuration from: {self.yaml_path}")
+
+ def imu_callback(self, data):
+ self.last_time_received = time.time()
+ self.last_offsets = [
+ data.linear_acceleration.x,
+ data.linear_acceleration.y,
+ data.linear_acceleration.z,
+ data.angular_velocity.x,
+ data.angular_velocity.y,
+ data.angular_velocity.z
+ ]
+
+ def save_offsets_to_yaml(self):
+ # Load the original YAML file
+ with open(self.yaml_path, 'r') as file:
+ data = yaml.safe_load(file)
+
+ # Update the offsets
+ data['axes_offsets'] = [int(offset) for offset in self.last_offsets]
+
+ # Generate the path for the new YAML file
+ new_yaml_path = os.path.join(os.path.dirname(self.yaml_path), 'new_mpu_settings.yaml')
+
+ # Save the updated data in a new YAML file using the custom settings
+ with open(new_yaml_path, 'w') as file:
+ yaml.dump(data, file, Dumper=FlowStyleListDumper, default_flow_style=None)
+
+ rospy.loginfo(f"Calibration complete. Offsets saved to: {new_yaml_path}")
+
+ def run(self):
+ rate = rospy.Rate(1) # Check every second
+ while not rospy.is_shutdown():
+ current_time = time.time()
+ if current_time - self.last_time_received > self.timeout_duration:
+ if self.last_offsets:
+ rospy.loginfo("No new messages from the calibration node. Assuming convergence.")
+ self.save_offsets_to_yaml()
+ rospy.signal_shutdown("Calibration Converged")
+ else:
+ rospy.logwarn("Timeout reached but no offsets received.")
+ else:
+ rospy.loginfo("Waiting for measurements to converge...")
+ rate.sleep()
+
+if __name__ == '__main__':
+ imu_cal_listener = IMUCalibrationListener()
+ imu_cal_listener.run()
diff --git a/src/mpu6050.cpp b/src/mpu6050.cpp
index 6be707b..0dcfadb 100755
--- a/src/mpu6050.cpp
+++ b/src/mpu6050.cpp
@@ -50,9 +50,9 @@ void MPU6050::initialize(const std::string& i2c_bus_uri) {
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
- setFullScaleGyroRange(MPU6050_GYRO_FS_250);
- setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
- setDLPFMode(MPU6050_DLPF_BW_10);
+ setFullScaleGyroRange(MPU6050_GYRO_FS_1000); // it was MPU6050_GYRO_FS_250
+ setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // it was MPU6050_ACCEL_FS_2
+ setDLPFMode(MPU6050_DLPF_BW_42); // it was MPU6050_DLPF_BW_10
accel_lsb_sensitivity_ = this->getAccelLSBSensitivity();
gyro_lsb_sensitivity_ = this->getGyroLSBSensitivity();
diff --git a/src/mpu6050_calibration_node.cpp b/src/mpu6050_calibration_node.cpp
index 77abdc6..23430fc 100644
--- a/src/mpu6050_calibration_node.cpp
+++ b/src/mpu6050_calibration_node.cpp
@@ -60,7 +60,7 @@ void MPU6050CalibrationNode::computeOffsets() {
float dt = 1.0 / pub_rate_; // How it isn't a dynamic system, sample time doesn't must exactly computed
IMUData imu_raw_data = mpu6050_.getRawMotion6();
- imu_raw_data.accel.z -= 16384; // Remove gravity contribution
+ imu_raw_data.accel.z -= 8192; // Remove gravity contribution 8192: +-4g, 16384: +-2g, 2048: +-8g, 2048: 16+-g
/* The divisions here is beacause the offsets need to be set when the MPU is
in the less sensitive mode (accel in 16g mode and gyro in 2000 degrees/sec mode).
diff --git a/src/mpu6050_calibration_node2.cpp b/src/mpu6050_calibration_node2.cpp
new file mode 100644
index 0000000..33c5b7b
--- /dev/null
+++ b/src/mpu6050_calibration_node2.cpp
@@ -0,0 +1,275 @@
+/* ============================================
+MIT License
+
+// Copyright (c) 2020 Mateus Meneses
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===============================================
+*/
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include "mpu6050_driver/mpu6050.hpp"
+
+class SensorCalibration {
+public:
+ SensorCalibration() {}
+
+ void addMeasurement(const Eigen::Vector3d& measured_acc, const Eigen::Vector3d& true_acc) {
+ measured_accs_.push_back(measured_acc);
+ true_accs_.push_back(true_acc);
+ }
+
+ void addGyroMeasurement(const Eigen::Vector3d& measured_gyro) {
+ measured_gyros_.push_back(measured_gyro);
+ }
+
+ void calibrate() {
+ int N = measured_accs_.size();
+ Eigen::MatrixXd Acc(3, N);
+ Eigen::MatrixXd TrueAcc(4, N);
+
+ for (int i = 0; i < N; ++i) {
+ Acc.col(i) = measured_accs_[i];
+ TrueAcc.col(i) << true_accs_[i], 1.0;
+ }
+
+ Eigen::MatrixXd TrueAccT = TrueAcc.transpose();
+ Eigen::MatrixXd product = Acc * TrueAccT;
+ Eigen::MatrixXd inv_product = (TrueAcc * TrueAccT).inverse();
+
+ Eigen::MatrixXd Unknowns = product * inv_product;
+
+ // Extract the offsets, gains, and cross-gains
+ Xgain_ = Unknowns(0, 0);
+ YtoX_ = Unknowns(0, 1);
+ ZtoX_ = Unknowns(0, 2);
+ Xofs_ = Unknowns(0, 3);
+
+ XtoY_ = Unknowns(1, 0);
+ Ygain_ = Unknowns(1, 1);
+ ZtoY_ = Unknowns(1, 2);
+ Yofs_ = Unknowns(1, 3);
+
+ XtoZ_ = Unknowns(2, 0);
+ YtoZ_ = Unknowns(2, 1);
+ Zgain_ = Unknowns(2, 2);
+ Zofs_ = Unknowns(2, 3);
+
+ A_ << Xgain_, YtoX_, ZtoX_,
+ XtoY_, Ygain_, ZtoY_,
+ XtoZ_, YtoZ_, Zgain_;
+
+ B_ << Xofs_, Yofs_, Zofs_;
+
+ // Compute gyro offsets
+ gyro_offsets_ = Eigen::Vector3d::Zero();
+ for (const auto& gyro : measured_gyros_) {
+ gyro_offsets_ += gyro;
+ }
+ gyro_offsets_ /= measured_gyros_.size();
+ }
+
+ void writeCalibrationToYAML(const std::string& filename) const {
+ YAML::Emitter out;
+ out << YAML::BeginMap;
+ out << YAML::Key << "bus_uri" << YAML::Value << "/dev/i2c-1";
+ out << YAML::Key << "mpu_address" << YAML::Value << "0x68";
+ out << YAML::Key << "pub_rate" << YAML::Value << 200;
+ out << YAML::Key << "frame_id" << YAML::Value << "imu";
+ out << YAML::Key << "axes_offsets" << YAML::Value << YAML::Flow << std::vector{0, 0, 0, 0, 0, 0};
+ out << YAML::Key << "ki" << YAML::Value << 0.2;
+ out << YAML::Key << "kp" << YAML::Value << 0.1;
+ out << YAML::Key << "delta" << YAML::Value << 0.2;
+
+ out << YAML::Key << "Xgain" << YAML::Value << Xgain_;
+ out << YAML::Key << "YtoX" << YAML::Value << YtoX_;
+ out << YAML::Key << "ZtoX" << YAML::Value << ZtoX_;
+ out << YAML::Key << "Xofs" << YAML::Value << Xofs_;
+ out << YAML::Key << "XtoY" << YAML::Value << XtoY_;
+ out << YAML::Key << "Ygain" << YAML::Value << Ygain_;
+ out << YAML::Key << "ZtoY" << YAML::Value << ZtoY_;
+ out << YAML::Key << "Yofs" << YAML::Value << Yofs_;
+ out << YAML::Key << "XtoZ" << YAML::Value << XtoZ_;
+ out << YAML::Key << "YtoZ" << YAML::Value << YtoZ_;
+ out << YAML::Key << "Zgain" << YAML::Value << Zgain_;
+ out << YAML::Key << "Zofs" << YAML::Value << Zofs_;
+
+ out << YAML::Key << "gyro_x_offset" << YAML::Value << gyro_offsets_.x();
+ out << YAML::Key << "gyro_y_offset" << YAML::Value << gyro_offsets_.y();
+ out << YAML::Key << "gyro_z_offset" << YAML::Value << gyro_offsets_.z();
+ out << YAML::EndMap;
+
+ std::ofstream fout(filename);
+ fout << out.c_str();
+ }
+
+ void printCalibration() {
+ std::cout << "Xgain: " << Xgain_ << ", YtoX: " << YtoX_ << ", ZtoX: " << ZtoX_ << ", Xofs: " << Xofs_ << std::endl;
+ std::cout << "XtoY: " << XtoY_ << ", Ygain: " << Ygain_ << ", ZtoY: " << ZtoY_ << ", Yofs: " << Yofs_ << std::endl;
+ std::cout << "XtoZ: " << XtoZ_ << ", YtoZ: " << YtoZ_ << ", Zgain: " << Zgain_ << ", Zofs: " << Zofs_ << std::endl;
+ std::cout << "Gyro offsets: " << gyro_offsets_.transpose() << std::endl;
+ }
+
+private:
+ std::vector measured_accs_;
+ std::vector true_accs_;
+ std::vector measured_gyros_;
+
+ double Xgain_, Ygain_, Zgain_;
+ double XtoY_, XtoZ_, YtoX_, YtoZ_, ZtoX_, ZtoY_;
+ double Xofs_, Yofs_, Zofs_;
+
+ Eigen::Vector3d gyro_offsets_;
+ Eigen::Matrix3d A_; // Matrix of gains and cross-axis gains
+ Eigen::Vector3d B_; // Vector of offsets
+};
+
+class MPU6050CalibrationNode {
+public:
+ MPU6050CalibrationNode() : nh_("~"), mpu6050_() {
+ loadParameters();
+ mpu6050_.setAddress(static_cast(mpu6050_addr_));
+ mpu6050_.initialize(i2c_bus_uri_);
+ mpu6050_.setDLPFMode(static_cast(3));
+ mpu6050_.setIntDataReadyEnabled(true);
+ }
+
+ void run() {
+ std::vector side_names{
+ "+X up", "-X up", "+Y up", "-Y up", "+Z up", "-Z up"
+ };
+
+ std::vector true_accs{
+ {1.0, 0.0, 0.0}, {-1.0, 0.0, 0.0},
+ {0.0, 1.0, 0.0}, {0.0, -1.0, 0.0},
+ {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0}
+ };
+
+ std::set used_sides;
+
+ while (ros::ok() && used_sides.size() < 6) {
+ std::cout << "Place the sensor in a new position and press Enter." << std::endl;
+ std::cin.ignore();
+
+ Eigen::Vector3d avg_measurement = collectMeasurements();
+
+ int side = detectSide(avg_measurement);
+ if (used_sides.find(side) != used_sides.end()) {
+ std::cout << "This side has already been used. Please use another side." << std::endl;
+ continue;
+ }
+
+ std::cout << "Collecting measurements for side " << side_names[side] << "." << std::endl;
+ std::cout << "Hold the current position for 2 seconds..." << std::endl;
+ ros::Duration(2.0).sleep(); // Wait for 2 seconds to ensure the IMU is stable
+
+ collectMeasurementsForSide(true_accs[side]);
+ used_sides.insert(side);
+ std::cout << "Measurements for side " << side_names[side] << " collected." << std::endl;
+ }
+
+ std::cout << "Now, place the sensor with +Z pointing up and keep it level. Press Enter to start gyroscope calibration." << std::endl;
+ std::cin.ignore();
+
+ collectGyroMeasurements();
+
+ if (ros::ok()) {
+ calibration_.calibrate();
+ calibration_.printCalibration();
+ calibration_.writeCalibrationToYAML(yaml_output_path_);
+ }
+
+ ros::shutdown();
+ }
+
+private:
+ ros::NodeHandle nh_;
+ std::string i2c_bus_uri_;
+ int mpu6050_addr_;
+ int num_measurements_;
+ std::string yaml_output_path_;
+ mpu6050_driver::MPU6050 mpu6050_;
+ SensorCalibration calibration_;
+
+ void loadParameters() {
+ nh_.param("bus_uri", i2c_bus_uri_, "/dev/i2c-1");
+ nh_.param("mpu_address", mpu6050_addr_, 0x68);
+ nh_.param("num_measurements", num_measurements_, 100);
+ nh_.param("yaml_output_path", yaml_output_path_, "");
+ }
+
+ Eigen::Vector3d collectMeasurements() {
+ std::vector measurements;
+ for (int i = 0; i < num_measurements_; ++i) {
+ if (mpu6050_.getIntDataReadyStatus()) {
+ mpu6050_driver::IMUData mpu_data = mpu6050_.getMotion6();
+ Eigen::Vector3d measured_acc(mpu_data.accel.x, mpu_data.accel.y, mpu_data.accel.z);
+ measurements.push_back(measured_acc);
+ ros::Duration(0.02).sleep(); // Adjust the delay as necessary
+ }
+ }
+ Eigen::Vector3d avg_measurement = Eigen::Vector3d::Zero();
+ for (const auto& measurement : measurements) {
+ avg_measurement += measurement;
+ }
+ avg_measurement /= measurements.size();
+ return avg_measurement;
+ }
+
+ void collectMeasurementsForSide(const Eigen::Vector3d& true_acc) {
+ for (int i = 0; i < num_measurements_; ++i) {
+ if (mpu6050_.getIntDataReadyStatus()) {
+ mpu6050_driver::IMUData mpu_data = mpu6050_.getMotion6();
+ Eigen::Vector3d measured_acc(mpu_data.accel.x, mpu_data.accel.y, mpu_data.accel.z);
+ calibration_.addMeasurement(measured_acc, true_acc);
+ ros::Duration(0.02).sleep(); // Adjust the delay as necessary
+ }
+ }
+ }
+
+ void collectGyroMeasurements() {
+ for (int i = 0; i < num_measurements_; ++i) {
+ if (mpu6050_.getIntDataReadyStatus()) {
+ mpu6050_driver::IMUData mpu_data = mpu6050_.getMotion6();
+ Eigen::Vector3d measured_gyro(mpu_data.gyro.x, mpu_data.gyro.y, mpu_data.gyro.z);
+ calibration_.addGyroMeasurement(measured_gyro);
+ ros::Duration(0.02).sleep(); // Adjust the delay as necessary
+ }
+ }
+ }
+
+ int detectSide(const Eigen::Vector3d& measurement) {
+ int max_index;
+ measurement.cwiseAbs().maxCoeff(&max_index);
+ double sign = measurement[max_index] > 0 ? 1.0 : -1.0;
+ return max_index * 2 + (sign < 0 ? 1 : 0);
+ }
+};
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "mpu6050_calibration_node");
+ MPU6050CalibrationNode node;
+ node.run();
+ return 0;
+}
diff --git a/src/mpu6050_node.cpp b/src/mpu6050_node.cpp
index 69c7369..d01ad92 100644
--- a/src/mpu6050_node.cpp
+++ b/src/mpu6050_node.cpp
@@ -42,7 +42,7 @@ void MPU6050Node::init() {
mpu6050_.setAddress(static_cast(mpu6050_addr_));
mpu6050_.initialize(i2c_bus_uri_);
- mpu6050_.setDLPFMode(static_cast(4));
+ mpu6050_.setDLPFMode(static_cast(3)); // it was 4
mpu6050_.setIntDataReadyEnabled(true);
this->setMPUOffsets();
diff --git a/src/mpu6050_node2.cpp b/src/mpu6050_node2.cpp
new file mode 100644
index 0000000..ed91dfa
--- /dev/null
+++ b/src/mpu6050_node2.cpp
@@ -0,0 +1,149 @@
+/* ============================================
+MIT License
+
+// Copyright (c) 2020 Mateus Meneses
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===============================================
+*/
+
+#include
+#include
+#include
+#include
+#include "mpu6050_driver/mpu6050.hpp"
+
+namespace mpu6050_driver {
+
+static const float gravity_value = 9.81;
+static const float deg_to_rad_factor = M_PI / 180.0;
+
+class MPU6050CorrectedNode {
+public:
+ MPU6050CorrectedNode() : nh_("~"), mpu6050_(), A_(Eigen::Matrix3d::Identity()), B_(Eigen::Vector3d::Zero()), gyro_offsets_(Eigen::Vector3d::Zero()) {
+ loadParameters();
+ if (!computeInverseCalibrationMatrix()) {
+ ROS_ERROR("Calibration matrix is not invertible. Please recalibrate the sensor.");
+ ros::shutdown();
+ }
+ mpu6050_.setAddress(static_cast(mpu6050_addr_));
+ mpu6050_.initialize(i2c_bus_uri_);
+ mpu6050_.setDLPFMode(static_cast(3));
+ mpu6050_.setIntDataReadyEnabled(true);
+ }
+
+ void init() {
+ mpu_data_pub_ = nh_.advertise("/imu/data_raw", 1);
+ ROS_INFO("MPU6050 Corrected Node has started");
+ }
+
+ void run() {
+ ros::Rate loop_rate(pub_rate_);
+
+ while (ros::ok()) {
+ ros::spinOnce();
+ if (mpu6050_.getIntDataReadyStatus()) this->publishCorrectedMPUData();
+ loop_rate.sleep();
+ }
+ }
+
+private:
+ ros::NodeHandle nh_;
+ std::string i2c_bus_uri_;
+ int mpu6050_addr_;
+ float pub_rate_;
+ std::string imu_frame_id_;
+ mpu6050_driver::MPU6050 mpu6050_;
+ ros::Publisher mpu_data_pub_;
+
+ Eigen::Matrix3d A_; // Calibration matrix
+ Eigen::Vector3d B_; // Offset vector
+ Eigen::Vector3d gyro_offsets_; // Gyroscope offsets
+
+ void loadParameters() {
+ nh_.param("bus_uri", i2c_bus_uri_, "/dev/i2c-1");
+ nh_.param("mpu_address", mpu6050_addr_, 0x68);
+ nh_.param("pub_rate", pub_rate_, 30);
+ nh_.param("frame_id", imu_frame_id_, "imu");
+
+ nh_.param("Xgain", A_(0, 0), 1.0);
+ nh_.param("YtoX", A_(0, 1), 0.0);
+ nh_.param("ZtoX", A_(0, 2), 0.0);
+ nh_.param("XtoY", A_(1, 0), 0.0);
+ nh_.param("Ygain", A_(1, 1), 1.0);
+ nh_.param("ZtoY", A_(1, 2), 0.0);
+ nh_.param("XtoZ", A_(2, 0), 0.0);
+ nh_.param("YtoZ", A_(2, 1), 0.0);
+ nh_.param("Zgain", A_(2, 2), 1.0);
+
+ nh_.param("Xofs", B_(0), 0.0);
+ nh_.param("Yofs", B_(1), 0.0);
+ nh_.param("Zofs", B_(2), 0.0);
+
+ nh_.param("gyro_x_offset", gyro_offsets_(0), 0.0);
+ nh_.param("gyro_y_offset", gyro_offsets_(1), 0.0);
+ nh_.param("gyro_z_offset", gyro_offsets_(2), 0.0);
+ }
+
+ bool computeInverseCalibrationMatrix() {
+ try {
+ A_ = A_.inverse();
+ return true;
+ } catch (const std::exception& e) {
+ ROS_ERROR("Failed to invert calibration matrix: %s", e.what());
+ return false;
+ }
+ }
+
+ void publishCorrectedMPUData() {
+ sensor_msgs::Imu imu_data;
+ IMUData mpu_data;
+
+ mpu_data = mpu6050_.getMotion6();
+
+ Eigen::Vector3d raw_acc(mpu_data.accel.x, mpu_data.accel.y, mpu_data.accel.z);
+ Eigen::Vector3d corrected_acc = A_ * (raw_acc - B_);
+
+ imu_data.linear_acceleration.x = corrected_acc.x() * gravity_value;
+ imu_data.linear_acceleration.y = corrected_acc.y() * gravity_value;
+ imu_data.linear_acceleration.z = corrected_acc.z() * gravity_value;
+
+ Eigen::Vector3d raw_gyro(mpu_data.gyro.x, mpu_data.gyro.y, mpu_data.gyro.z);
+ Eigen::Vector3d corrected_gyro = raw_gyro - gyro_offsets_;
+
+ imu_data.angular_velocity.x = corrected_gyro.x() * deg_to_rad_factor;
+ imu_data.angular_velocity.y = corrected_gyro.y() * deg_to_rad_factor;
+ imu_data.angular_velocity.z = corrected_gyro.z() * deg_to_rad_factor;
+
+ imu_data.header.frame_id = imu_frame_id_;
+ imu_data.header.stamp = ros::Time::now();
+
+ mpu_data_pub_.publish(imu_data);
+ }
+};
+
+} // namespace mpu6050_driver
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "mpu6050_node");
+ mpu6050_driver::MPU6050CorrectedNode node;
+ node.init();
+ node.run();
+ return 0;
+}