├── include
    └── mpu6050driver
    │   ├── mpu6050driver.h
    │   └── mpu6050sensor.h
├── package.xml
├── params
    └── mpu6050.yaml
├── CMakeLists.txt
├── launch
    └── mpu6050driver_launch.py
├── README.md
└── src
    ├── mpu6050driver.cpp
    └── mpu6050sensor.cpp


/include/mpu6050driver/mpu6050driver.h:
--------------------------------------------------------------------------------
 1 | #ifndef MPU6050DRIVER_H
 2 | #define MPU6050DRIVER_H
 3 | 
 4 | #include "mpu6050driver/mpu6050sensor.h"
 5 | #include "rclcpp/rclcpp.hpp"
 6 | #include "sensor_msgs/msg/imu.hpp"
 7 | 
 8 | class MPU6050Driver : public rclcpp::Node {
 9 |  public:
10 |   MPU6050Driver();
11 | 
12 |  private:
13 |   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
14 |   std::unique_ptr<MPU6050Sensor> mpu6050_;
15 |   size_t count_;
16 |   rclcpp::TimerBase::SharedPtr timer_;
17 |   void handleInput();
18 |   void declareParameters();
19 | };
20 | 
21 | #endif  // MPU6050DRIVER_H


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>mpu6050driver</name>
 5 |   <version>0.0.1</version>
 6 |   <description>A ROS2 package that converts raw data from an MPU6050 to /imu format.</description>
 7 |   <maintainer email="hiwad.aziz@gmx.de">Hiwad Aziz</maintainer>
 8 |   <license>MIT</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <test_depend>ament_lint_auto</test_depend>
13 |   <test_depend>ament_lint_common</test_depend>
14 | 
15 |   <export>
16 |     <build_type>ament_cmake</build_type>
17 |   </export>
18 | </package>
19 | 


--------------------------------------------------------------------------------
/params/mpu6050.yaml:
--------------------------------------------------------------------------------
 1 | mpu6050driver_node:
 2 |   ros__parameters:
 3 |     calibrate: True
 4 |     # Gyroscope range: 0 -> +-250°/s, 1 -> +-500°/s, 2 -> +-1000°/s, 3 -> +-2000°/s
 5 |     gyro_range: 0
 6 |     # Acceleration range: 0 -> +-2g, 1 -> +-4g, 2 -> +-8g, 3 -> +-16g
 7 |     accel_range: 0
 8 |     # Digital low pass filter bandwidth [0-6]: 0 -> 260Hz, 1 -> 184Hz, 2 -> 94Hz, 3 -> 44Hz, 4 -> 21Hz, 5 -> 10Hz, 6 -> 5Hz
 9 |     dlpf_bandwidth: 2
10 |     #If "calibrate" is true, these values will be overriden by the calibration procedure
11 |     gyro_x_offset: 0.0 # [deg/s]
12 |     gyro_y_offset: 0.0 # [deg/s]
13 |     gyro_z_offset: 0.0 # [deg/s]
14 |     accel_x_offset: 0.0 # [m/s²]
15 |     accel_y_offset: 0.0 # [m/s²]
16 |     accel_z_offset: 0.0 # [m/s²]
17 |     frequency: 100 # [Hz]
18 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | project(mpu6050driver)
 3 | 
 4 | # Default to C99
 5 | if(NOT CMAKE_C_STANDARD)
 6 |   set(CMAKE_C_STANDARD 99)
 7 | endif()
 8 | 
 9 | # Default to C++14
10 | if(NOT CMAKE_CXX_STANDARD)
11 |   set(CMAKE_CXX_STANDARD 14)
12 | endif()
13 | 
14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
15 |   add_compile_options(-Wall -Wextra -Wpedantic)
16 | endif()
17 | 
18 | # find dependencies
19 | find_package(ament_cmake REQUIRED)
20 | find_package(rclcpp REQUIRED)
21 | find_package(sensor_msgs REQUIRED)
22 | 
23 | add_executable(mpu6050driver src/mpu6050driver.cpp src/mpu6050sensor.cpp)
24 | ament_target_dependencies(mpu6050driver rclcpp sensor_msgs)
25 | target_link_libraries(mpu6050driver i2c)
26 | target_include_directories(
27 |   mpu6050driver PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
28 |                        $<INSTALL_INTERFACE:include>)
29 | 
30 | install(TARGETS mpu6050driver DESTINATION lib/${PROJECT_NAME})
31 | install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}/)
32 | ament_package()
33 | 


--------------------------------------------------------------------------------
/launch/mpu6050driver_launch.py:
--------------------------------------------------------------------------------
 1 | from ament_index_python.packages import get_package_share_directory
 2 | from launch import LaunchDescription
 3 | from launch_ros.actions import Node
 4 | from launch.actions import DeclareLaunchArgument
 5 | from launch.substitutions import LaunchConfiguration
 6 | 
 7 | import os
 8 | 
 9 | 
10 | def generate_launch_description():
11 |     ld = LaunchDescription()
12 |     share_dir = get_package_share_directory('mpu6050driver')
13 |     parameter_file = LaunchConfiguration('params_file')
14 | 
15 |     params_declare = DeclareLaunchArgument('params_file',
16 |                                            default_value=os.path.join(
17 |                                                share_dir, 'params', 'mpu6050.yaml'),
18 |                                            description='Path to the ROS2 parameters file to use.')
19 | 
20 |     mpu6050driver_node = Node(
21 |         package='mpu6050driver',
22 |         executable='mpu6050driver',
23 |         name='mpu6050driver_node',
24 |         output="screen",
25 |         emulate_tty=True,
26 |         parameters=[parameter_file]
27 |     )
28 | 
29 |     ld.add_action(params_declare)
30 |     ld.add_action(mpu6050driver_node)
31 |     return ld
32 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | 
 2 | # MPU6050 Driver for ROS2
 3 | This repository contains a ROS2 package that interfaces with an MPU6050 sensor over I2C. The sensor is calibrated on node startup (sensor needs to be on a plane with z-axis up and should not be moved during calibration). Calibration can be turned off in the parameters file. The output is an IMU ROS message but the quaternion part is currently set to zero.
 4 | 
 5 | ## Dependencies
 6 | -  libi2c-dev
 7 | 
 8 | ## Setup
 9 | The number of iterations for calibration can be set up in `include/mpu6050driver/mpu6050sensor.h`.
10 | Following other parameters and default values are listed here and can be changed in `params/mpu6050.yaml`.
11 | ```    calibrate: True
12 |     gyro_range: 0
13 |     accel_range: 0
14 |     dlpf_bandwidth: 2
15 |     gyro_x_offset: 0.0 # [deg/s]
16 |     gyro_y_offset: 0.0 # [deg/s]
17 |     gyro_z_offset: 0.0 # [deg/s]
18 |     accel_x_offset: 0.0 # [m/s²]
19 |     accel_y_offset: 0.0 # [m/s²]
20 |     accel_z_offset: 0.0 # [m/s²]
21 |     frequency: 100 # [Hz]
22 | ```
23 | 
24 | Build the package in your workspace:
25 | 
26 |     colcon build --packages-select mpu6050driver
27 | 
28 | Source setup.bash in your workspace:
29 | 
30 |     . install/setup.bash
31 |     
32 | Launch it:
33 | 
34 |     ros2 launch mpu6050driver mpu6050driver_launch.py
35 | 
36 | 


--------------------------------------------------------------------------------
/include/mpu6050driver/mpu6050sensor.h:
--------------------------------------------------------------------------------
 1 | #ifndef MPU6050SENSOR_H
 2 | #define MPU6050SENSOR_H
 3 | 
 4 | #include <string>
 5 | #include <unordered_map>
 6 | 
 7 | class MPU6050Sensor {
 8 |  public:
 9 |   MPU6050Sensor(int bus_number = 1);
10 |   ~MPU6050Sensor();
11 | 
12 |   enum AccelRange { ACC_2_G, ACC_4_G, ACC_8_G, ACC_16_G };
13 |   enum GyroRange { GYR_250_DEG_S, GYR_500_DEG_S, GYR_1000_DEG_S, GYR_2000_DEG_S };
14 |   enum DlpfBandwidth {
15 |     DLPF_260_HZ,
16 |     DLPF_184_HZ,
17 |     DLPF_94_HZ,
18 |     DLPF_44_HZ,
19 |     DLPF_21_HZ,
20 |     DLPF_10_HZ,
21 |     DLPF_5_HZ
22 |   };
23 | 
24 |   void printConfig() const;
25 |   void printOffsets() const;
26 |   void setGyroscopeRange(GyroRange range);
27 |   void setAccelerometerRange(AccelRange range);
28 |   void setDlpfBandwidth(DlpfBandwidth bandwidth);
29 |   double getAccelerationX() const;
30 |   double getAccelerationY() const;
31 |   double getAccelerationZ() const;
32 |   double getAngularVelocityX() const;
33 |   double getAngularVelocityY() const;
34 |   double getAngularVelocityZ() const;
35 |   void setGyroscopeOffset(double gyro_x_offset, double gyro_y_offset, double gyro_z_offset);
36 |   void setAccelerometerOffset(double accel_x_offset, double accel_y_offset, double accel_z_offset);
37 |   void calibrate();
38 | 
39 |  private:
40 |   double convertRawGyroscopeData(int16_t gyro_raw_) const;
41 |   double convertRawAccelerometerData(int16_t accel_raw_) const;
42 |   int readGyroscopeRange();
43 |   int readAccelerometerRange();
44 |   int readDlpfConfig();
45 |   void reportError(int error);
46 | 
47 |   int file_;
48 |   char filename_[10] = "/dev/i2c-";
49 |   int accel_range_{2};
50 |   int gyro_range_{250};
51 |   int dlpf_range_{260};
52 |   bool calibrated_{false};
53 |   double gyro_x_offset_{0.0};
54 |   double gyro_y_offset_{0.0};
55 |   double gyro_z_offset_{0.0};
56 |   double accel_x_offset_{0.0};
57 |   double accel_y_offset_{0.0};
58 |   double accel_z_offset_{0.0};
59 | 
60 |   // MPU6050 registers and addresses (s. datasheet for details)
61 |   static constexpr int MPU6050_ADDRESS_DEFAULT = 0x68;
62 |   static constexpr int PWR_MGMT_1 = 0x6B;
63 |   static constexpr int GYRO_CONFIG = 0x1B;
64 |   static constexpr int ACCEL_CONFIG = 0x1C;
65 |   static constexpr int ACCEL_XOUT_H = 0x3B;
66 |   static constexpr int ACCEL_YOUT_H = 0x3D;
67 |   static constexpr int ACCEL_ZOUT_H = 0x3F;
68 |   static constexpr int GYRO_XOUT_H = 0x43;
69 |   static constexpr int GYRO_YOUT_H = 0x45;
70 |   static constexpr int GYRO_ZOUT_H = 0x47;
71 |   static constexpr int DLPF_CONFIG = 0x1A;
72 |   // Helper constants
73 |   static constexpr int GYRO_CONFIG_SHIFT = 3;
74 |   static constexpr int ACCEL_CONFIG_SHIFT = 3;
75 |   static constexpr double GRAVITY = 9.81;
76 |   const std::array<int, 4> ACCEL_RANGES{2, 4, 8, 16};
77 |   const std::array<int, 4> GYRO_RANGES{250, 500, 1000, 2000};
78 |   const std::array<int, 7> DLPF_RANGES{260, 184, 94, 44, 21, 10, 5};
79 |   const std::unordered_map<int, int> ACCEL_SENS_MAP{{2, 16384}, {4, 8192}, {8, 4096}, {16, 2048}};
80 |   const std::unordered_map<int, double> GYRO_SENS_MAP{
81 |       {250, 131}, {500, 65.5}, {1000, 32.8}, {2000, 16.4}};
82 |   static constexpr int CALIBRATION_COUNT{200};
83 | };
84 | 
85 | #endif  // MPU6050SENSOR_H
86 | 


--------------------------------------------------------------------------------
/src/mpu6050driver.cpp:
--------------------------------------------------------------------------------
 1 | #include "mpu6050driver/mpu6050driver.h"
 2 | 
 3 | #include <chrono>
 4 | #include <memory>
 5 | 
 6 | using namespace std::chrono_literals;
 7 | 
 8 | MPU6050Driver::MPU6050Driver()
 9 |     : Node("mpu6050publisher"), mpu6050_{std::make_unique<MPU6050Sensor>()}
10 | {
11 |   // Declare parameters
12 |   declareParameters();
13 |   // Set parameters
14 |   mpu6050_->setGyroscopeRange(
15 |       static_cast<MPU6050Sensor::GyroRange>(this->get_parameter("gyro_range").as_int()));
16 |   mpu6050_->setAccelerometerRange(
17 |       static_cast<MPU6050Sensor::AccelRange>(this->get_parameter("accel_range").as_int()));
18 |   mpu6050_->setDlpfBandwidth(
19 |       static_cast<MPU6050Sensor::DlpfBandwidth>(this->get_parameter("dlpf_bandwidth").as_int()));
20 |   mpu6050_->setGyroscopeOffset(this->get_parameter("gyro_x_offset").as_double(),
21 |                                this->get_parameter("gyro_y_offset").as_double(),
22 |                                this->get_parameter("gyro_z_offset").as_double());
23 |   mpu6050_->setAccelerometerOffset(this->get_parameter("accel_x_offset").as_double(),
24 |                                    this->get_parameter("accel_y_offset").as_double(),
25 |                                    this->get_parameter("accel_z_offset").as_double());
26 |   // Check if we want to calibrate the sensor
27 |   if (this->get_parameter("calibrate").as_bool()) {
28 |     RCLCPP_INFO(this->get_logger(), "Calibrating...");
29 |     mpu6050_->calibrate();
30 |   }
31 |   mpu6050_->printConfig();
32 |   mpu6050_->printOffsets();
33 |   // Create publisher
34 |   publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
35 |   std::chrono::duration<int64_t, std::milli> frequency =
36 |       1000ms / this->get_parameter("gyro_range").as_int();
37 |   timer_ = this->create_wall_timer(frequency, std::bind(&MPU6050Driver::handleInput, this));
38 | }
39 | 
40 | void MPU6050Driver::handleInput()
41 | {
42 |   auto message = sensor_msgs::msg::Imu();
43 |   message.header.stamp = this->get_clock()->now();
44 |   message.header.frame_id = "base_link";
45 |   message.linear_acceleration_covariance = {0};
46 |   message.linear_acceleration.x = mpu6050_->getAccelerationX();
47 |   message.linear_acceleration.y = mpu6050_->getAccelerationY();
48 |   message.linear_acceleration.z = mpu6050_->getAccelerationZ();
49 |   message.angular_velocity_covariance[0] = {0};
50 |   message.angular_velocity.x = mpu6050_->getAngularVelocityX();
51 |   message.angular_velocity.y = mpu6050_->getAngularVelocityY();
52 |   message.angular_velocity.z = mpu6050_->getAngularVelocityZ();
53 |   // Invalidate quaternion
54 |   message.orientation_covariance[0] = -1;
55 |   message.orientation.x = 0;
56 |   message.orientation.y = 0;
57 |   message.orientation.z = 0;
58 |   message.orientation.w = 0;
59 |   publisher_->publish(message);
60 | }
61 | 
62 | void MPU6050Driver::declareParameters()
63 | {
64 |   this->declare_parameter<bool>("calibrate", true);
65 |   this->declare_parameter<int>("gyro_range", MPU6050Sensor::GyroRange::GYR_250_DEG_S);
66 |   this->declare_parameter<int>("accel_range", MPU6050Sensor::AccelRange::ACC_2_G);
67 |   this->declare_parameter<int>("dlpf_bandwidth", MPU6050Sensor::DlpfBandwidth::DLPF_260_HZ);
68 |   this->declare_parameter<double>("gyro_x_offset", 0.0);
69 |   this->declare_parameter<double>("gyro_y_offset", 0.0);
70 |   this->declare_parameter<double>("gyro_z_offset", 0.0);
71 |   this->declare_parameter<double>("accel_x_offset", 0.0);
72 |   this->declare_parameter<double>("accel_y_offset", 0.0);
73 |   this->declare_parameter<double>("accel_z_offset", 0.0);
74 |   this->declare_parameter<int>("frequency", 0.0);
75 | }
76 | 
77 | int main(int argc, char* argv[])
78 | {
79 |   rclcpp::init(argc, argv);
80 |   rclcpp::spin(std::make_shared<MPU6050Driver>());
81 |   rclcpp::shutdown();
82 |   return 0;
83 | }


--------------------------------------------------------------------------------
/src/mpu6050sensor.cpp:
--------------------------------------------------------------------------------
  1 | #include "mpu6050driver/mpu6050sensor.h"
  2 | 
  3 | extern "C" {
  4 | #include <errno.h>
  5 | #include <fcntl.h>
  6 | #include <i2c/smbus.h>
  7 | #include <linux/i2c-dev.h>
  8 | #include <string.h>
  9 | #include <sys/ioctl.h>
 10 | #include <unistd.h>
 11 | }
 12 | 
 13 | #include <iostream>
 14 | 
 15 | MPU6050Sensor::MPU6050Sensor(int bus_number)
 16 | {
 17 |   // TODO: make char append cleaner
 18 |   filename_[9] = *std::to_string(bus_number).c_str();
 19 |   std::cout << filename_ << std::endl;
 20 |   file_ = open(filename_, O_RDWR);
 21 |   if (file_ < 0) {
 22 |     std::cerr << "Failed to open file descriptor! Check your bus number! Errno: "
 23 |               << strerror(errno);
 24 |     exit(1);
 25 |   }
 26 |   if (ioctl(file_, I2C_SLAVE, MPU6050_ADDRESS_DEFAULT) < 0) {
 27 |     std::cerr << "Failed to find device address! Check device address!";
 28 |     exit(1);
 29 |   }
 30 |   // Wake up sensor
 31 |   int result = i2c_smbus_write_byte_data(file_, PWR_MGMT_1, 0);
 32 |   if (result < 0) reportError(errno);
 33 |   // Read current ranges from sensor
 34 |   readGyroscopeRange();
 35 |   readAccelerometerRange();
 36 |   readDlpfConfig();
 37 | }
 38 | 
 39 | MPU6050Sensor::~MPU6050Sensor() { close(file_); }
 40 | 
 41 | void MPU6050Sensor::printConfig() const
 42 | {
 43 |   std::cout << "Accelerometer Range: +-" << accel_range_ << "g\n";
 44 |   std::cout << "Gyroscope Range: +-" << gyro_range_ << " degree per sec\n";
 45 |   std::cout << "DLPF Range: " << dlpf_range_ << " Hz\n";
 46 | }
 47 | 
 48 | void MPU6050Sensor::printOffsets() const
 49 | {
 50 |   std::cout << "Accelerometer Offsets: x: " << accel_x_offset_ << ", y: " << accel_y_offset_
 51 |             << ", z: " << accel_z_offset_ << "\n";
 52 |   std::cout << "Gyroscope Offsets: x: " << gyro_x_offset_ << ", y: " << gyro_y_offset_
 53 |             << ", z: " << gyro_z_offset_ << "\n";
 54 | }
 55 | 
 56 | int MPU6050Sensor::readGyroscopeRange()
 57 | {
 58 |   int range = i2c_smbus_read_byte_data(file_, GYRO_CONFIG);
 59 |   if (range < 0) reportError(errno);
 60 |   range = range >> GYRO_CONFIG_SHIFT;
 61 |   gyro_range_ = GYRO_RANGES[range];
 62 |   return gyro_range_;
 63 | }
 64 | 
 65 | int MPU6050Sensor::readAccelerometerRange()
 66 | {
 67 |   int range = i2c_smbus_read_byte_data(file_, ACCEL_CONFIG);
 68 |   if (range < 0) reportError(errno);
 69 |   range = range >> ACCEL_CONFIG_SHIFT;
 70 |   accel_range_ = ACCEL_RANGES[range];
 71 |   return accel_range_;
 72 | }
 73 | 
 74 | int MPU6050Sensor::readDlpfConfig()
 75 | {
 76 |   int range = i2c_smbus_read_byte_data(file_, DLPF_CONFIG);
 77 |   if (range < 0) reportError(errno);
 78 |   range = range & 7;  // Read only first 3 bits
 79 |   dlpf_range_ = DLPF_RANGES[range];
 80 |   return dlpf_range_;
 81 | }
 82 | 
 83 | void MPU6050Sensor::setGyroscopeRange(MPU6050Sensor::GyroRange range)
 84 | {
 85 |   int result = i2c_smbus_write_byte_data(file_, GYRO_CONFIG, range << GYRO_CONFIG_SHIFT);
 86 |   if (result < 0) reportError(errno);
 87 |   gyro_range_ = GYRO_RANGES[static_cast<size_t>(range)];
 88 | }
 89 | 
 90 | void MPU6050Sensor::setAccelerometerRange(MPU6050Sensor::AccelRange range)
 91 | {
 92 |   int result = i2c_smbus_write_byte_data(file_, ACCEL_CONFIG, range << ACCEL_CONFIG_SHIFT);
 93 |   if (result < 0) reportError(errno);
 94 |   accel_range_ = ACCEL_RANGES[static_cast<size_t>(range)];
 95 | }
 96 | 
 97 | void MPU6050Sensor::setDlpfBandwidth(DlpfBandwidth bandwidth)
 98 | {
 99 |   int result = i2c_smbus_write_byte_data(file_, DLPF_CONFIG, bandwidth);
100 |   if (result < 0) reportError(errno);
101 |   dlpf_range_ = DLPF_RANGES[static_cast<size_t>(bandwidth)];
102 | }
103 | 
104 | double MPU6050Sensor::getAccelerationX() const
105 | {
106 |   int16_t accel_x_msb = i2c_smbus_read_byte_data(file_, ACCEL_XOUT_H);
107 |   int16_t accel_x_lsb = i2c_smbus_read_byte_data(file_, ACCEL_XOUT_H + 1);
108 |   int16_t accel_x = accel_x_lsb | accel_x_msb << 8;
109 |   double accel_x_converted = convertRawAccelerometerData(accel_x);
110 |   if (calibrated_) {
111 |     return accel_x_converted - accel_x_offset_;
112 |   }
113 |   return accel_x_converted;
114 | }
115 | 
116 | double MPU6050Sensor::getAccelerationY() const
117 | {
118 |   int16_t accel_y_msb = i2c_smbus_read_byte_data(file_, ACCEL_YOUT_H);
119 |   int16_t accel_y_lsb = i2c_smbus_read_byte_data(file_, ACCEL_YOUT_H + 1);
120 |   int16_t accel_y = accel_y_lsb | accel_y_msb << 8;
121 |   double accel_y_converted = convertRawAccelerometerData(accel_y);
122 |   if (calibrated_) {
123 |     return accel_y_converted - accel_y_offset_;
124 |   }
125 |   return accel_y_converted;
126 | }
127 | 
128 | double MPU6050Sensor::getAccelerationZ() const
129 | {
130 |   int16_t accel_z_msb = i2c_smbus_read_byte_data(file_, ACCEL_ZOUT_H);
131 |   int16_t accel_z_lsb = i2c_smbus_read_byte_data(file_, ACCEL_ZOUT_H + 1);
132 |   int16_t accel_z = accel_z_lsb | accel_z_msb << 8;
133 |   double accel_z_converted = convertRawAccelerometerData(accel_z);
134 |   if (calibrated_) {
135 |     return accel_z_converted - accel_z_offset_;
136 |   }
137 |   return accel_z_converted;
138 | }
139 | 
140 | double MPU6050Sensor::getAngularVelocityX() const
141 | {
142 |   int16_t gyro_x_msb = i2c_smbus_read_byte_data(file_, GYRO_XOUT_H);
143 |   int16_t gyro_x_lsb = i2c_smbus_read_byte_data(file_, GYRO_XOUT_H + 1);
144 |   int16_t gyro_x = gyro_x_lsb | gyro_x_msb << 8;
145 |   double gyro_x_converted = convertRawGyroscopeData(gyro_x);
146 |   if (calibrated_) {
147 |     return gyro_x_converted - gyro_x_offset_;
148 |   }
149 |   return gyro_x_converted;
150 | }
151 | 
152 | double MPU6050Sensor::getAngularVelocityY() const
153 | {
154 |   int16_t gyro_y_msb = i2c_smbus_read_byte_data(file_, GYRO_YOUT_H);
155 |   int16_t gyro_y_lsb = i2c_smbus_read_byte_data(file_, GYRO_YOUT_H + 1);
156 |   int16_t gyro_y = gyro_y_lsb | gyro_y_msb << 8;
157 |   double gyro_y_converted = convertRawGyroscopeData(gyro_y);
158 |   if (calibrated_) {
159 |     return gyro_y_converted - gyro_y_offset_;
160 |   }
161 |   return gyro_y_converted;
162 | }
163 | 
164 | double MPU6050Sensor::getAngularVelocityZ() const
165 | {
166 |   int16_t gyro_z_msb = i2c_smbus_read_byte_data(file_, GYRO_ZOUT_H);
167 |   int16_t gyro_z_lsb = i2c_smbus_read_byte_data(file_, GYRO_ZOUT_H + 1);
168 |   int16_t gyro_z = gyro_z_lsb | gyro_z_msb << 8;
169 |   double gyro_z_converted = convertRawGyroscopeData(gyro_z);
170 |   if (calibrated_) {
171 |     return gyro_z_converted - gyro_z_offset_;
172 |   }
173 |   return gyro_z_converted;
174 | }
175 | 
176 | double MPU6050Sensor::convertRawGyroscopeData(int16_t gyro_raw) const
177 | {
178 |   const double ang_vel_in_deg_per_s = static_cast<double>(gyro_raw) / GYRO_SENS_MAP.at(gyro_range_);
179 |   return ang_vel_in_deg_per_s;
180 | }
181 | 
182 | double MPU6050Sensor::convertRawAccelerometerData(int16_t accel_raw) const
183 | {
184 |   const double accel_in_m_per_s =
185 |       static_cast<double>(accel_raw) / ACCEL_SENS_MAP.at(accel_range_) * GRAVITY;
186 |   return accel_in_m_per_s;
187 | }
188 | 
189 | void MPU6050Sensor::setGyroscopeOffset(double gyro_x_offset, double gyro_y_offset,
190 |                                        double gyro_z_offset)
191 | {
192 |   gyro_x_offset_ = gyro_x_offset;
193 |   gyro_y_offset_ = gyro_y_offset;
194 |   gyro_z_offset_ = gyro_z_offset;
195 | }
196 | 
197 | void MPU6050Sensor::setAccelerometerOffset(double accel_x_offset, double accel_y_offset,
198 |                                            double accel_z_offset)
199 | {
200 |   accel_x_offset_ = accel_x_offset;
201 |   accel_y_offset_ = accel_y_offset;
202 |   accel_z_offset_ = accel_z_offset;
203 | }
204 | 
205 | void MPU6050Sensor::calibrate()
206 | {
207 |   int count = 0;
208 |   while (count < CALIBRATION_COUNT) {
209 |     gyro_x_offset_ += getAngularVelocityX();
210 |     gyro_y_offset_ += getAngularVelocityY();
211 |     gyro_z_offset_ += getAngularVelocityZ();
212 |     accel_x_offset_ += getAccelerationX();
213 |     accel_y_offset_ += getAccelerationY();
214 |     accel_z_offset_ += getAccelerationZ();
215 |     ++count;
216 |   }
217 |   gyro_x_offset_ /= CALIBRATION_COUNT;
218 |   gyro_y_offset_ /= CALIBRATION_COUNT;
219 |   gyro_z_offset_ /= CALIBRATION_COUNT;
220 |   accel_x_offset_ /= CALIBRATION_COUNT;
221 |   accel_y_offset_ /= CALIBRATION_COUNT;
222 |   accel_z_offset_ /= CALIBRATION_COUNT;
223 |   accel_z_offset_ -= GRAVITY;
224 |   calibrated_ = true;
225 | }
226 | 
227 | void MPU6050Sensor::reportError(int error) { std::cerr << "Error! Errno: " << strerror(error); }


--------------------------------------------------------------------------------