diff --git a/lib/MPU9250/.gitignore b/lib/MPU9250/.gitignore
new file mode 100644
index 0000000..56bef8c
--- /dev/null
+++ b/lib/MPU9250/.gitignore
@@ -0,0 +1 @@
+examples/
\ No newline at end of file
diff --git a/lib/MPU9250/LICENSE b/lib/MPU9250/LICENSE
new file mode 100644
index 0000000..93cff99
--- /dev/null
+++ b/lib/MPU9250/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Hideaki Tai
+
+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.
diff --git a/lib/MPU9250/MPU9250.h b/lib/MPU9250/MPU9250.h
new file mode 100644
index 0000000..8175986
--- /dev/null
+++ b/lib/MPU9250/MPU9250.h
@@ -0,0 +1,1110 @@
+#pragma once
+#ifndef MPU9250_H
+#define MPU9250_H
+
+#include <Wire.h>
+
+#include "mpu9250/MPU9250RegisterMap.h"
+#include "mpu9250/QuaternionFilter.h"
+
+enum class ACCEL_FS_SEL
+{
+    A2G,
+    A4G,
+    A8G,
+    A16G
+};
+enum class GYRO_FS_SEL
+{
+    G250DPS,
+    G500DPS,
+    G1000DPS,
+    G2000DPS
+};
+enum class MAG_OUTPUT_BITS
+{
+    M14BITS,
+    M16BITS
+};
+
+enum class FIFO_SAMPLE_RATE : uint8_t
+{
+    SMPL_1000HZ,
+    SMPL_500HZ,
+    SMPL_333HZ,
+    SMPL_250HZ,
+    SMPL_200HZ,
+    SMPL_167HZ,
+    SMPL_143HZ,
+    SMPL_125HZ,
+};
+
+enum class GYRO_DLPF_CFG : uint8_t
+{
+    DLPF_250HZ,
+    DLPF_184HZ,
+    DLPF_92HZ,
+    DLPF_41HZ,
+    DLPF_20HZ,
+    DLPF_10HZ,
+    DLPF_5HZ,
+    DLPF_3600HZ,
+};
+
+enum class ACCEL_DLPF_CFG : uint8_t
+{
+    DLPF_218HZ_0,
+    DLPF_218HZ_1,
+    DLPF_99HZ,
+    DLPF_45HZ,
+    DLPF_21HZ,
+    DLPF_10HZ,
+    DLPF_5HZ,
+    DLPF_420HZ,
+};
+
+static constexpr uint8_t MPU9250_WHOAMI_DEFAULT_VALUE{0x71};
+static constexpr uint8_t MPU9255_WHOAMI_DEFAULT_VALUE{0x73};
+static constexpr uint8_t MPU6500_WHOAMI_DEFAULT_VALUE{0x70};
+
+struct MPU9250Setting
+{
+    ACCEL_FS_SEL accel_fs_sel{ACCEL_FS_SEL::A16G};
+    GYRO_FS_SEL gyro_fs_sel{GYRO_FS_SEL::G2000DPS};
+    MAG_OUTPUT_BITS mag_output_bits{MAG_OUTPUT_BITS::M16BITS};
+    FIFO_SAMPLE_RATE fifo_sample_rate{FIFO_SAMPLE_RATE::SMPL_200HZ};
+    uint8_t gyro_fchoice{0x03};
+    GYRO_DLPF_CFG gyro_dlpf_cfg{GYRO_DLPF_CFG::DLPF_41HZ};
+    uint8_t accel_fchoice{0x01};
+    ACCEL_DLPF_CFG accel_dlpf_cfg{ACCEL_DLPF_CFG::DLPF_45HZ};
+};
+
+template <typename WireType>
+class MPU9250_
+{
+    static constexpr uint8_t MPU9250_DEFAULT_ADDRESS{0x68}; // Device address when ADO = 0
+    static constexpr uint8_t AK8963_ADDRESS{0x0C};          //  Address of magnetometer
+    static constexpr uint8_t AK8963_WHOAMI_DEFAULT_VALUE{0x48};
+    uint8_t mpu_i2c_addr{MPU9250_DEFAULT_ADDRESS};
+
+    // settings
+    MPU9250Setting setting;
+    // TODO: this should be configured!!
+    static constexpr uint8_t MAG_MODE{0x06}; // 0x02 for 8 Hz, 0x06 for 100 Hz continuous magnetometer data read
+    float acc_resolution{0.f};               // scale resolutions per LSB for the sensors
+    float gyro_resolution{0.f};              // scale resolutions per LSB for the sensors
+    float mag_resolution{0.f};               // scale resolutions per LSB for the sensors
+
+    // Calibration Parameters
+    float acc_bias[3]{0., 0., 0.};  // acc calibration value in ACCEL_FS_SEL: 2g
+    float gyro_bias[3]{0., 0., 0.}; // gyro calibration value in GYRO_FS_SEL: 250dps
+    float mag_bias_factory[3]{0., 0., 0.};
+    float mag_bias[3]{0., 0., 0.}; // mag calibration value in MAG_OUTPUT_BITS: 16BITS
+    float mag_scale[3]{1., 1., 1.};
+    float magnetic_declination = -7.51; // Japan, 24th June
+
+    // Temperature
+    int16_t temperature_count{0}; // temperature raw count output
+    float temperature{0.f};       // Stores the real internal chip temperature in degrees Celsius
+
+    // Self Test
+    float self_test_result[6]{0.f}; // holds results of gyro and accelerometer self test
+
+    // IMU Data
+    float a[3]{0.f, 0.f, 0.f};
+    float g[3]{0.f, 0.f, 0.f};
+    float m[3]{0.f, 0.f, 0.f};
+    float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
+    float rpy[3]{0.f, 0.f, 0.f};
+    float lin_acc[3]{0.f, 0.f, 0.f}; // linear acceleration (acceleration with gravity component subtracted)
+    QuaternionFilter quat_filter;
+    size_t n_filter_iter{1};
+
+    // Other settings
+    bool has_connected{false};
+    bool b_ahrs{true};
+    bool b_verbose{false};
+
+    // I2C
+    WireType *wire;
+    uint8_t i2c_err_;
+
+public:
+    static constexpr uint16_t CALIB_GYRO_SENSITIVITY{131};    // LSB/degrees/sec
+    static constexpr uint16_t CALIB_ACCEL_SENSITIVITY{16384}; // LSB/g
+
+    bool setup(const uint8_t addr, const MPU9250Setting &mpu_setting = MPU9250Setting(), WireType &w = Wire)
+    {
+        // addr should be valid for MPU
+        if ((addr < MPU9250_DEFAULT_ADDRESS) || (addr > MPU9250_DEFAULT_ADDRESS + 7))
+        {
+            Serial.print("I2C address 0x");
+            Serial.print(addr, HEX);
+            Serial.println(" is not valid for MPU. Please check your I2C address.");
+            return false;
+        }
+        mpu_i2c_addr = addr;
+        setting = mpu_setting;
+        wire = &w;
+
+        if (isConnectedMPU9250())
+        {
+            initMPU9250();
+            if (isConnectedAK8963())
+                initAK8963();
+            else
+            {
+                if (b_verbose)
+                    Serial.println("Could not connect to AK8963");
+                has_connected = false;
+                return false;
+            }
+        }
+        else
+        {
+            if (b_verbose)
+                Serial.println("Could not connect to MPU9250");
+            has_connected = false;
+            return false;
+        }
+        has_connected = true;
+        return true;
+    }
+
+    void sleep(bool b)
+    {
+        byte c = read_byte(mpu_i2c_addr, PWR_MGMT_1); // read the value, change sleep bit to match b, write byte back to register
+        if (b)
+        {
+            c = c | 0x40; // sets the sleep bit
+        }
+        else
+        {
+            c = c & 0xBF; // mask 1011111 keeps all the previous bits
+        }
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, c);
+    }
+
+    void verbose(const bool b)
+    {
+        b_verbose = b;
+    }
+
+    void ahrs(const bool b)
+    {
+        b_ahrs = b;
+    }
+
+    void calibrateAccelGyro()
+    {
+        calibrate_acc_gyro_impl();
+    }
+
+    void calibrateMag()
+    {
+        calibrate_mag_impl();
+    }
+
+    bool isConnected()
+    {
+        has_connected = isConnectedMPU9250() && isConnectedAK8963();
+        return has_connected;
+    }
+
+    bool isConnectedMPU9250()
+    {
+        byte c = read_byte(mpu_i2c_addr, WHO_AM_I_MPU9250);
+        if (b_verbose)
+        {
+            Serial.print("MPU9250 WHO AM I = ");
+            Serial.println(c, HEX);
+        }
+        bool b = (c == MPU9250_WHOAMI_DEFAULT_VALUE);
+        b |= (c == MPU9255_WHOAMI_DEFAULT_VALUE);
+        b |= (c == MPU6500_WHOAMI_DEFAULT_VALUE);
+        return b;
+    }
+
+    bool isConnectedAK8963()
+    {
+        byte c = read_byte(AK8963_ADDRESS, AK8963_WHO_AM_I);
+        if (b_verbose)
+        {
+            Serial.print("AK8963 WHO AM I = ");
+            Serial.println(c, HEX);
+        }
+        return (c == AK8963_WHOAMI_DEFAULT_VALUE);
+    }
+
+    bool isSleeping()
+    {
+        byte c = read_byte(mpu_i2c_addr, PWR_MGMT_1);
+        return (c & 0x40) == 0x40;
+    }
+
+    bool available()
+    {
+        return has_connected && (read_byte(mpu_i2c_addr, INT_STATUS) & 0x01);
+    }
+
+    bool update()
+    {
+        if (!available())
+            return false;
+
+        update_accel_gyro();
+        update_mag();
+
+        // Madgwick function needs to be fed North, East, and Down direction like
+        // (AN, AE, AD, GN, GE, GD, MN, ME, MD)
+        // Accel and Gyro direction is Right-Hand, X-Forward, Z-Up
+        // Magneto direction is Right-Hand, Y-Forward, Z-Down
+        // So to adopt to the general Aircraft coordinate system (Right-Hand, X-Forward, Z-Down),
+        // we need to feed (ax, -ay, -az, gx, -gy, -gz, my, -mx, mz)
+        // but we pass (-ax, ay, az, gx, -gy, -gz, my, -mx, mz)
+        // because gravity is by convention positive down, we need to ivnert the accel data
+
+        // get quaternion based on aircraft coordinate (Right-Hand, X-Forward, Z-Down)
+        // acc[mg], gyro[deg/s], mag [mG]
+        // gyro will be convert from [deg/s] to [rad/s] inside of this function
+        // quat_filter.update(-a[0], a[1], a[2], g[0] * DEG_TO_RAD, -g[1] * DEG_TO_RAD, -g[2] * DEG_TO_RAD, m[1], -m[0], m[2], q);
+
+        float an = -a[0];
+        float ae = +a[1];
+        float ad = +a[2];
+        float gn = +g[0] * DEG_TO_RAD;
+        float ge = -g[1] * DEG_TO_RAD;
+        float gd = -g[2] * DEG_TO_RAD;
+        float mn = +m[1];
+        float me = -m[0];
+        float md = +m[2];
+
+        for (size_t i = 0; i < n_filter_iter; ++i)
+        {
+            quat_filter.update(an, ae, ad, gn, ge, gd, mn, me, md, q);
+        }
+
+        if (!b_ahrs)
+        {
+            temperature_count = read_temperature_data();              // Read the adc values
+            temperature = ((float)temperature_count) / 333.87 + 21.0; // Temperature in degrees Centigrade
+        }
+        else
+        {
+            update_rpy(q[0], q[1], q[2], q[3]);
+        }
+        return true;
+    }
+
+    float getRoll() const { return rpy[0]; }
+    float getPitch() const { return rpy[1]; }
+    float getYaw() const { return rpy[2]; }
+
+    float getEulerX() const { return rpy[0]; }
+    float getEulerY() const { return -rpy[1]; }
+    float getEulerZ() const { return -rpy[2]; }
+
+    float getQuaternionX() const { return q[1]; }
+    float getQuaternionY() const { return q[2]; }
+    float getQuaternionZ() const { return q[3]; }
+    float getQuaternionW() const { return q[0]; }
+
+    float getAcc(const uint8_t i) const { return (i < 3) ? a[i] : 0.f; }
+    float getGyro(const uint8_t i) const { return (i < 3) ? g[i] : 0.f; }
+    float getMag(const uint8_t i) const { return (i < 3) ? m[i] : 0.f; }
+    float getLinearAcc(const uint8_t i) const { return (i < 3) ? lin_acc[i] : 0.f; }
+
+    float getAccX() const { return a[0]; }
+    float getAccY() const { return a[1]; }
+    float getAccZ() const { return a[2]; }
+    float getGyroX() const { return g[0]; }
+    float getGyroY() const { return g[1]; }
+    float getGyroZ() const { return g[2]; }
+    float getMagX() const { return m[0]; }
+    float getMagY() const { return m[1]; }
+    float getMagZ() const { return m[2]; }
+    float getLinearAccX() const { return lin_acc[0]; }
+    float getLinearAccY() const { return lin_acc[1]; }
+    float getLinearAccZ() const { return lin_acc[2]; }
+
+    float getAccBias(const uint8_t i) const { return (i < 3) ? acc_bias[i] : 0.f; }
+    float getGyroBias(const uint8_t i) const { return (i < 3) ? gyro_bias[i] : 0.f; }
+    float getMagBias(const uint8_t i) const { return (i < 3) ? mag_bias[i] : 0.f; }
+    float getMagScale(const uint8_t i) const { return (i < 3) ? mag_scale[i] : 0.f; }
+
+    float getAccBiasX() const { return acc_bias[0]; }
+    float getAccBiasY() const { return acc_bias[1]; }
+    float getAccBiasZ() const { return acc_bias[2]; }
+    float getGyroBiasX() const { return gyro_bias[0]; }
+    float getGyroBiasY() const { return gyro_bias[1]; }
+    float getGyroBiasZ() const { return gyro_bias[2]; }
+    float getMagBiasX() const { return mag_bias[0]; }
+    float getMagBiasY() const { return mag_bias[1]; }
+    float getMagBiasZ() const { return mag_bias[2]; }
+    float getMagScaleX() const { return mag_scale[0]; }
+    float getMagScaleY() const { return mag_scale[1]; }
+    float getMagScaleZ() const { return mag_scale[2]; }
+
+    float getTemperature() const { return temperature; }
+
+    void setAccBias(const float x, const float y, const float z)
+    {
+        acc_bias[0] = x;
+        acc_bias[1] = y;
+        acc_bias[2] = z;
+        write_accel_offset();
+    }
+    void setGyroBias(const float x, const float y, const float z)
+    {
+        gyro_bias[0] = x;
+        gyro_bias[1] = y;
+        gyro_bias[2] = z;
+        write_gyro_offset();
+    }
+    void setMagBias(const float x, const float y, const float z)
+    {
+        mag_bias[0] = x;
+        mag_bias[1] = y;
+        mag_bias[2] = z;
+    }
+    void setMagScale(const float x, const float y, const float z)
+    {
+        mag_scale[0] = x;
+        mag_scale[1] = y;
+        mag_scale[2] = z;
+    }
+    void setMagneticDeclination(const float d) { magnetic_declination = d; }
+
+    void selectFilter(QuatFilterSel sel)
+    {
+        quat_filter.select_filter(sel);
+    }
+
+    void setFilterIterations(const size_t n)
+    {
+        if (n > 0)
+            n_filter_iter = n;
+    }
+
+    bool selftest()
+    {
+        return self_test_impl();
+    }
+
+private:
+    void initMPU9250()
+    {
+        acc_resolution = get_acc_resolution(setting.accel_fs_sel);
+        gyro_resolution = get_gyro_resolution(setting.gyro_fs_sel);
+        mag_resolution = get_mag_resolution(setting.mag_output_bits);
+
+        // reset device
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+        delay(100);
+
+        // wake up device
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
+        delay(100);                                 // Wait for all registers to reset
+
+        // get stable time source
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
+        delay(200);
+
+        // Configure Gyro and Thermometer
+        // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
+        // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
+        // be higher than 1 / 0.0059 = 170 Hz
+        // GYRO_DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
+        // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
+        uint8_t mpu_config = (uint8_t)setting.gyro_dlpf_cfg;
+        write_byte(mpu_i2c_addr, MPU_CONFIG, mpu_config);
+
+        // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
+        uint8_t sample_rate = (uint8_t)setting.fifo_sample_rate;
+        write_byte(mpu_i2c_addr, SMPLRT_DIV, sample_rate); // Use a 200 Hz rate; a rate consistent with the filter update rate
+                                                           // determined inset in CONFIG above
+
+        // Set gyroscope full scale range
+        // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
+        uint8_t c = read_byte(mpu_i2c_addr, GYRO_CONFIG); // get current GYRO_CONFIG register value
+        c = c & ~0xE0;                                    // Clear self-test bits [7:5]
+        c = c & ~0x03;                                    // Clear Fchoice bits [1:0]
+        c = c & ~0x18;                                    // Clear GYRO_FS_SEL bits [4:3]
+        c = c | (uint8_t(setting.gyro_fs_sel) << 3);      // Set full scale range for the gyro
+        c = c | (uint8_t(~setting.gyro_fchoice) & 0x03);  // Set Fchoice for the gyro
+        write_byte(mpu_i2c_addr, GYRO_CONFIG, c);         // Write new GYRO_CONFIG value to register
+
+        // Set accelerometer full-scale range configuration
+        c = read_byte(mpu_i2c_addr, ACCEL_CONFIG);    // get current ACCEL_CONFIG register value
+        c = c & ~0xE0;                                // Clear self-test bits [7:5]
+        c = c & ~0x18;                                // Clear ACCEL_FS_SEL bits [4:3]
+        c = c | (uint8_t(setting.accel_fs_sel) << 3); // Set full scale range for the accelerometer
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG, c);    // Write new ACCEL_CONFIG register value
+
+        // Set accelerometer sample rate configuration
+        // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
+        // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
+        c = read_byte(mpu_i2c_addr, ACCEL_CONFIG2);       // get current ACCEL_CONFIG2 register value
+        c = c & ~0x0F;                                    // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
+        c = c | (~(setting.accel_fchoice << 3) & 0x08);   // Set accel_fchoice_b to 1
+        c = c | (uint8_t(setting.accel_dlpf_cfg) & 0x07); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG2, c);       // Write new ACCEL_CONFIG2 register value
+
+        // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
+        // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
+
+        // Configure Interrupts and Bypass Enable
+        // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
+        // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
+        // can join the I2C bus and all can be controlled by the Arduino as master
+        write_byte(mpu_i2c_addr, INT_PIN_CFG, 0x22);
+        write_byte(mpu_i2c_addr, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
+        delay(100);
+    }
+
+    void initAK8963()
+    {
+        // First extract the factory calibration for each magnetometer axis
+        uint8_t raw_data[3];                           // x/y/z gyro calibration data stored here
+        write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+        delay(10);
+        write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
+        delay(10);
+        read_bytes(AK8963_ADDRESS, AK8963_ASAX, 3, &raw_data[0]);     // Read the x-, y-, and z-axis calibration values
+        mag_bias_factory[0] = (float)(raw_data[0] - 128) / 256. + 1.; // Return x-axis sensitivity adjustment values, etc.
+        mag_bias_factory[1] = (float)(raw_data[1] - 128) / 256. + 1.;
+        mag_bias_factory[2] = (float)(raw_data[2] - 128) / 256. + 1.;
+        write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+        delay(10);
+        // Configure the magnetometer for continuous read and highest resolution
+        // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
+        // and enable continuous mode data acquisition MAG_MODE (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
+        write_byte(AK8963_ADDRESS, AK8963_CNTL, (uint8_t)setting.mag_output_bits << 4 | MAG_MODE); // Set magnetometer data resolution and sample ODR
+        delay(10);
+
+        if (b_verbose)
+        {
+            Serial.println("Mag Factory Calibration Values: ");
+            Serial.print("X-Axis sensitivity offset value ");
+            Serial.println(mag_bias_factory[0], 2);
+            Serial.print("Y-Axis sensitivity offset value ");
+            Serial.println(mag_bias_factory[1], 2);
+            Serial.print("Z-Axis sensitivity offset value ");
+            Serial.println(mag_bias_factory[2], 2);
+        }
+    }
+
+public:
+    void update_rpy(float qw, float qx, float qy, float qz)
+    {
+        // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+        // In this coordinate system, the positive z-axis is down toward Earth.
+        // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
+        // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+        // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+        // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+        // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+        // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+        // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+        float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components
+        a12 = 2.0f * (qx * qy + qw * qz);
+        a22 = qw * qw + qx * qx - qy * qy - qz * qz;
+        a31 = 2.0f * (qw * qx + qy * qz);
+        a32 = 2.0f * (qx * qz - qw * qy);
+        a33 = qw * qw - qx * qx - qy * qy + qz * qz;
+        rpy[0] = atan2f(a31, a33);
+        rpy[1] = -asinf(a32);
+        rpy[2] = atan2f(a12, a22);
+        rpy[0] *= 180.0f / PI;
+        rpy[1] *= 180.0f / PI;
+        rpy[2] *= 180.0f / PI;
+        rpy[2] += magnetic_declination;
+        if (rpy[2] >= +180.f)
+            rpy[2] -= 360.f;
+        else if (rpy[2] < -180.f)
+            rpy[2] += 360.f;
+
+        lin_acc[0] = a[0] + a31;
+        lin_acc[1] = a[1] + a32;
+        lin_acc[2] = a[2] - a33;
+    }
+
+    void update_accel_gyro()
+    {
+        int16_t raw_acc_gyro_data[7];       // used to read all 14 bytes at once from the MPU9250 accel/gyro
+        read_accel_gyro(raw_acc_gyro_data); // INT cleared on any read
+
+        // Now we'll calculate the accleration value into actual g's
+        a[0] = (float)raw_acc_gyro_data[0] * acc_resolution; // get actual g value, this depends on scale being set
+        a[1] = (float)raw_acc_gyro_data[1] * acc_resolution;
+        a[2] = (float)raw_acc_gyro_data[2] * acc_resolution;
+
+        temperature_count = raw_acc_gyro_data[3];                 // Read the adc values
+        temperature = ((float)temperature_count) / 333.87 + 21.0; // Temperature in degrees Centigrade
+
+        // Calculate the gyro value into actual degrees per second
+        g[0] = (float)raw_acc_gyro_data[4] * gyro_resolution; // get actual gyro value, this depends on scale being set
+        g[1] = (float)raw_acc_gyro_data[5] * gyro_resolution;
+        g[2] = (float)raw_acc_gyro_data[6] * gyro_resolution;
+    }
+
+private:
+    void read_accel_gyro(int16_t *destination)
+    {
+        uint8_t raw_data[14];                                                // x/y/z accel register data stored here
+        read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 14, &raw_data[0]);            // Read the 14 raw data registers into data array
+        destination[0] = ((int16_t)raw_data[0] << 8) | (int16_t)raw_data[1]; // Turn the MSB and LSB into a signed 16-bit value
+        destination[1] = ((int16_t)raw_data[2] << 8) | (int16_t)raw_data[3];
+        destination[2] = ((int16_t)raw_data[4] << 8) | (int16_t)raw_data[5];
+        destination[3] = ((int16_t)raw_data[6] << 8) | (int16_t)raw_data[7];
+        destination[4] = ((int16_t)raw_data[8] << 8) | (int16_t)raw_data[9];
+        destination[5] = ((int16_t)raw_data[10] << 8) | (int16_t)raw_data[11];
+        destination[6] = ((int16_t)raw_data[12] << 8) | (int16_t)raw_data[13];
+    }
+
+public:
+    void update_mag()
+    {
+        int16_t mag_count[3] = {0, 0, 0}; // Stores the 16-bit signed magnetometer sensor output
+
+        // Read the x/y/z adc values
+        if (read_mag(mag_count))
+        {
+            // Calculate the magnetometer values in milliGauss
+            // Include factory calibration per data sheet and user environmental corrections
+            // mag_bias is calcurated in 16BITS
+            float bias_to_current_bits = mag_resolution / get_mag_resolution(MAG_OUTPUT_BITS::M16BITS);
+            m[0] = (float)(mag_count[0] * mag_resolution * mag_bias_factory[0] - mag_bias[0] * bias_to_current_bits) * mag_scale[0]; // get actual magnetometer value, this depends on scale being set
+            m[1] = (float)(mag_count[1] * mag_resolution * mag_bias_factory[1] - mag_bias[1] * bias_to_current_bits) * mag_scale[1];
+            m[2] = (float)(mag_count[2] * mag_resolution * mag_bias_factory[2] - mag_bias[2] * bias_to_current_bits) * mag_scale[2];
+        }
+    }
+
+private:
+    bool read_mag(int16_t *destination)
+    {
+        const uint8_t st1 = read_byte(AK8963_ADDRESS, AK8963_ST1);
+        if (st1 & 0x01)
+        {                                                               // wait for magnetometer data ready bit to be set
+            uint8_t raw_data[7];                                        // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
+            read_bytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &raw_data[0]); // Read the six raw data and ST2 registers sequentially into data array
+            if (MAG_MODE == 0x02 || MAG_MODE == 0x04 || MAG_MODE == 0x06)
+            {                          // continuous or external trigger read mode
+                if ((st1 & 0x02) != 0) // check if data is not skipped
+                    return false;      // this should be after data reading to clear DRDY register
+            }
+
+            uint8_t c = raw_data[6]; // End data read by reading ST2 register
+            if (!(c & 0x08))
+            {                                                               // Check if magnetic sensor overflow set, if not then report data
+                destination[0] = ((int16_t)raw_data[1] << 8) | raw_data[0]; // Turn the MSB and LSB into a signed 16-bit value
+                destination[1] = ((int16_t)raw_data[3] << 8) | raw_data[2]; // Data stored as little Endian
+                destination[2] = ((int16_t)raw_data[5] << 8) | raw_data[4];
+                return true;
+            }
+        }
+        return false;
+    }
+
+    int16_t read_temperature_data()
+    {
+        uint8_t raw_data[2];                                   // x/y/z gyro register data stored here
+        read_bytes(mpu_i2c_addr, TEMP_OUT_H, 2, &raw_data[0]); // Read the two raw data registers sequentially into data array
+        return ((int16_t)raw_data[0] << 8) | raw_data[1];      // Turn the MSB and LSB into a 16-bit value
+    }
+
+    // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
+    // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
+    // ACCEL_FS_SEL: 2g (maximum sensitivity)
+    // GYRO_FS_SEL: 250dps (maximum sensitivity)
+    void calibrate_acc_gyro_impl()
+    {
+        set_acc_gyro_to_calibration();
+        collect_acc_gyro_data_to(acc_bias, gyro_bias);
+        write_accel_offset();
+        write_gyro_offset();
+        delay(100);
+        initMPU9250();
+        delay(1000);
+    }
+
+    void set_acc_gyro_to_calibration()
+    {
+        // reset device
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+        delay(100);
+
+        // get stable time source; Auto select clock source to be PLL gyroscope reference if ready
+        // else use the internal oscillator, bits 2:0 = 001
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x01);
+        write_byte(mpu_i2c_addr, PWR_MGMT_2, 0x00);
+        delay(200);
+
+        // Configure device for bias calculation
+        write_byte(mpu_i2c_addr, INT_ENABLE, 0x00);   // Disable all interrupts
+        write_byte(mpu_i2c_addr, FIFO_EN, 0x00);      // Disable FIFO
+        write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x00);   // Turn on internal clock source
+        write_byte(mpu_i2c_addr, I2C_MST_CTRL, 0x00); // Disable I2C master
+        write_byte(mpu_i2c_addr, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
+        write_byte(mpu_i2c_addr, USER_CTRL, 0x0C);    // Reset FIFO and DMP
+        delay(15);
+
+        // Configure MPU6050 gyro and accelerometer for bias calculation
+        write_byte(mpu_i2c_addr, MPU_CONFIG, 0x01);   // Set low-pass filter to 188 Hz
+        write_byte(mpu_i2c_addr, SMPLRT_DIV, 0x00);   // Set sample rate to 1 kHz
+        write_byte(mpu_i2c_addr, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+
+        // Configure FIFO to capture accelerometer and gyro data for bias calculation
+        write_byte(mpu_i2c_addr, USER_CTRL, 0x40); // Enable FIFO
+        write_byte(mpu_i2c_addr, FIFO_EN, 0x78);   // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)
+        delay(40);                                 // accumulate 40 samples in 40 milliseconds = 480 bytes
+    }
+
+    void collect_acc_gyro_data_to(float *a_bias, float *g_bias)
+    {
+        // At end of sample accumulation, turn off FIFO sensor read
+        uint8_t data[12];                                   // data array to hold accelerometer and gyro x, y, z, data
+        write_byte(mpu_i2c_addr, FIFO_EN, 0x00);            // Disable gyro and accelerometer sensors for FIFO
+        read_bytes(mpu_i2c_addr, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
+        uint16_t fifo_count = ((uint16_t)data[0] << 8) | data[1];
+        uint16_t packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
+
+        for (uint16_t ii = 0; ii < packet_count; ii++)
+        {
+            int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+            read_bytes(mpu_i2c_addr, FIFO_R_W, 12, &data[0]);             // read data for averaging
+            accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO
+            accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
+            accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
+            gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
+            gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
+            gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
+
+            a_bias[0] += (float)accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+            a_bias[1] += (float)accel_temp[1];
+            a_bias[2] += (float)accel_temp[2];
+            g_bias[0] += (float)gyro_temp[0];
+            g_bias[1] += (float)gyro_temp[1];
+            g_bias[2] += (float)gyro_temp[2];
+        }
+        a_bias[0] /= (float)packet_count; // Normalize sums to get average count biases
+        a_bias[1] /= (float)packet_count;
+        a_bias[2] /= (float)packet_count;
+        g_bias[0] /= (float)packet_count;
+        g_bias[1] /= (float)packet_count;
+        g_bias[2] /= (float)packet_count;
+
+        if (a_bias[2] > 0L)
+        {
+            a_bias[2] -= (float)CALIB_ACCEL_SENSITIVITY;
+        } // Remove gravity from the z-axis accelerometer bias calculation
+        else
+        {
+            a_bias[2] += (float)CALIB_ACCEL_SENSITIVITY;
+        }
+    }
+
+    void write_accel_offset()
+    {
+        // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
+        // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
+        // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
+        // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
+        // the accelerometer biases calculated above must be divided by 8.
+
+        uint8_t read_data[2] = {0};
+        int16_t acc_bias_reg[3] = {0, 0, 0};                     // A place to hold the factory accelerometer trim biases
+        read_bytes(mpu_i2c_addr, XA_OFFSET_H, 2, &read_data[0]); // Read factory accelerometer trim values
+        acc_bias_reg[0] = ((int16_t)read_data[0] << 8) | read_data[1];
+        read_bytes(mpu_i2c_addr, YA_OFFSET_H, 2, &read_data[0]);
+        acc_bias_reg[1] = ((int16_t)read_data[0] << 8) | read_data[1];
+        read_bytes(mpu_i2c_addr, ZA_OFFSET_H, 2, &read_data[0]);
+        acc_bias_reg[2] = ((int16_t)read_data[0] << 8) | read_data[1];
+
+        int16_t mask_bit[3] = {1, 1, 1}; // Define array to hold mask bit for each accelerometer bias axis
+        for (int i = 0; i < 3; i++)
+        {
+            if (acc_bias_reg[i] % 2)
+            {
+                mask_bit[i] = 0;
+            }
+            acc_bias_reg[i] -= (int16_t)acc_bias[i] >> 3; // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
+            if (mask_bit[i])
+            {
+                acc_bias_reg[i] = acc_bias_reg[i] & ~mask_bit[i]; // Preserve temperature compensation bit
+            }
+            else
+            {
+                acc_bias_reg[i] = acc_bias_reg[i] | 0x0001; // Preserve temperature compensation bit
+            }
+        }
+
+        uint8_t write_data[6] = {0};
+        write_data[0] = (acc_bias_reg[0] >> 8) & 0xFF;
+        write_data[1] = (acc_bias_reg[0]) & 0xFF;
+        write_data[2] = (acc_bias_reg[1] >> 8) & 0xFF;
+        write_data[3] = (acc_bias_reg[1]) & 0xFF;
+        write_data[4] = (acc_bias_reg[2] >> 8) & 0xFF;
+        write_data[5] = (acc_bias_reg[2]) & 0xFF;
+
+        // Push accelerometer biases to hardware registers
+        write_byte(mpu_i2c_addr, XA_OFFSET_H, write_data[0]);
+        write_byte(mpu_i2c_addr, XA_OFFSET_L, write_data[1]);
+        write_byte(mpu_i2c_addr, YA_OFFSET_H, write_data[2]);
+        write_byte(mpu_i2c_addr, YA_OFFSET_L, write_data[3]);
+        write_byte(mpu_i2c_addr, ZA_OFFSET_H, write_data[4]);
+        write_byte(mpu_i2c_addr, ZA_OFFSET_L, write_data[5]);
+    }
+
+    void write_gyro_offset()
+    {
+        // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
+        uint8_t gyro_offset_data[6]{0};
+        gyro_offset_data[0] = (-(int16_t)gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+        gyro_offset_data[1] = (-(int16_t)gyro_bias[0] / 4) & 0xFF;      // Biases are additive, so change sign on calculated average gyro biases
+        gyro_offset_data[2] = (-(int16_t)gyro_bias[1] / 4 >> 8) & 0xFF;
+        gyro_offset_data[3] = (-(int16_t)gyro_bias[1] / 4) & 0xFF;
+        gyro_offset_data[4] = (-(int16_t)gyro_bias[2] / 4 >> 8) & 0xFF;
+        gyro_offset_data[5] = (-(int16_t)gyro_bias[2] / 4) & 0xFF;
+
+        // Push gyro biases to hardware registers
+        write_byte(mpu_i2c_addr, XG_OFFSET_H, gyro_offset_data[0]);
+        write_byte(mpu_i2c_addr, XG_OFFSET_L, gyro_offset_data[1]);
+        write_byte(mpu_i2c_addr, YG_OFFSET_H, gyro_offset_data[2]);
+        write_byte(mpu_i2c_addr, YG_OFFSET_L, gyro_offset_data[3]);
+        write_byte(mpu_i2c_addr, ZG_OFFSET_H, gyro_offset_data[4]);
+        write_byte(mpu_i2c_addr, ZG_OFFSET_L, gyro_offset_data[5]);
+    }
+
+    // mag calibration is executed in MAG_OUTPUT_BITS: 16BITS
+    void calibrate_mag_impl()
+    {
+        // set MAG_OUTPUT_BITS to maximum to calibrate
+        MAG_OUTPUT_BITS mag_output_bits_cache = setting.mag_output_bits;
+        setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
+        initAK8963();
+        collect_mag_data_to(mag_bias, mag_scale);
+
+        if (b_verbose)
+        {
+            Serial.println("Mag Calibration done!");
+
+            Serial.println("AK8963 mag biases (mG)");
+            Serial.print(mag_bias[0]);
+            Serial.print(", ");
+            Serial.print(mag_bias[1]);
+            Serial.print(", ");
+            Serial.print(mag_bias[2]);
+            Serial.println();
+            Serial.println("AK8963 mag scale (mG)");
+            Serial.print(mag_scale[0]);
+            Serial.print(", ");
+            Serial.print(mag_scale[1]);
+            Serial.print(", ");
+            Serial.print(mag_scale[2]);
+            Serial.println();
+        }
+
+        // restore MAG_OUTPUT_BITS
+        setting.mag_output_bits = mag_output_bits_cache;
+        initAK8963();
+    }
+
+    void collect_mag_data_to(float *m_bias, float *m_scale)
+    {
+        if (b_verbose)
+            Serial.println("Mag Calibration: Wave device in a figure eight until done!");
+        delay(4000);
+
+        // shoot for ~fifteen seconds of mag data
+        uint16_t sample_count = 0;
+        if (MAG_MODE == 0x02)
+            sample_count = 128;    // at 8 Hz ODR, new mag data is available every 125 ms
+        else if (MAG_MODE == 0x06) // in this library, fixed to 100Hz
+            sample_count = 1500;   // at 100 Hz ODR, new mag data is available every 10 ms
+
+        int32_t bias[3] = {0, 0, 0}, scale[3] = {0, 0, 0};
+        int16_t mag_max[3] = {-32767, -32767, -32767};
+        int16_t mag_min[3] = {32767, 32767, 32767};
+        int16_t mag_temp[3] = {0, 0, 0};
+        for (uint16_t ii = 0; ii < sample_count; ii++)
+        {
+            read_mag(mag_temp); // Read the mag data
+            for (int jj = 0; jj < 3; jj++)
+            {
+                if (mag_temp[jj] > mag_max[jj])
+                    mag_max[jj] = mag_temp[jj];
+                if (mag_temp[jj] < mag_min[jj])
+                    mag_min[jj] = mag_temp[jj];
+            }
+            if (MAG_MODE == 0x02)
+                delay(135); // at 8 Hz ODR, new mag data is available every 125 ms
+            if (MAG_MODE == 0x06)
+                delay(12); // at 100 Hz ODR, new mag data is available every 10 ms
+        }
+
+        if (b_verbose)
+        {
+            Serial.println("mag x min/max:");
+            Serial.println(mag_min[0]);
+            Serial.println(mag_max[0]);
+            Serial.println("mag y min/max:");
+            Serial.println(mag_min[1]);
+            Serial.println(mag_max[1]);
+            Serial.println("mag z min/max:");
+            Serial.println(mag_min[2]);
+            Serial.println(mag_max[2]);
+        }
+
+        // Get hard iron correction
+        bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts
+        bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts
+        bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts
+
+        float bias_resolution = get_mag_resolution(MAG_OUTPUT_BITS::M16BITS);
+        m_bias[0] = (float)bias[0] * bias_resolution * mag_bias_factory[0]; // save mag biases in G for main program
+        m_bias[1] = (float)bias[1] * bias_resolution * mag_bias_factory[1];
+        m_bias[2] = (float)bias[2] * bias_resolution * mag_bias_factory[2];
+
+        // Get soft iron correction estimate
+        //*** multiplication by mag_bias_factory added in accordance with the following comment
+        //*** https://github.com/kriswiner/MPU9250/issues/456#issue-836657973
+        scale[0] = (float)(mag_max[0] - mag_min[0]) * mag_bias_factory[0] / 2; // get average x axis max chord length in counts
+        scale[1] = (float)(mag_max[1] - mag_min[1]) * mag_bias_factory[1] / 2; // get average y axis max chord length in counts
+        scale[2] = (float)(mag_max[2] - mag_min[2]) * mag_bias_factory[2] / 2; // get average z axis max chord length in counts
+
+        float avg_rad = scale[0] + scale[1] + scale[2];
+        avg_rad /= 3.0;
+
+        m_scale[0] = avg_rad / ((float)scale[0]);
+        m_scale[1] = avg_rad / ((float)scale[1]);
+        m_scale[2] = avg_rad / ((float)scale[2]);
+    }
+
+    // Accelerometer and gyroscope self test; check calibration wrt factory settings
+    bool self_test_impl() // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
+    {
+        uint8_t raw_data[6] = {0, 0, 0, 0, 0, 0};
+        int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
+        float factoryTrim[6];
+        uint8_t FS = 0;
+
+        write_byte(mpu_i2c_addr, SMPLRT_DIV, 0x00);      // Set gyro sample rate to 1 kHz
+        write_byte(mpu_i2c_addr, MPU_CONFIG, 0x02);      // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
+        write_byte(mpu_i2c_addr, GYRO_CONFIG, FS << 3);  // Set full scale range for the gyro to 250 dps
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG2, 0x02);   // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG, FS << 3); // Set full scale range for the accelerometer to 2 g
+
+        for (int ii = 0; ii < 200; ii++)
+        { // get average current values of gyro and acclerometer
+
+            read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 6, &raw_data[0]);         // Read the six raw data registers into data array
+            aAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value
+            aAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
+            aAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
+
+            read_bytes(mpu_i2c_addr, GYRO_XOUT_H, 6, &raw_data[0]);          // Read the six raw data registers sequentially into data array
+            gAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value
+            gAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
+            gAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
+        }
+
+        for (int ii = 0; ii < 3; ii++)
+        { // Get average of 200 values and store as average current readings
+            aAvg[ii] /= 200;
+            gAvg[ii] /= 200;
+        }
+
+        // Configure the accelerometer for self-test
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
+        write_byte(mpu_i2c_addr, GYRO_CONFIG, 0xE0);  // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+        delay(25);                                    // Delay a while to let the device stabilize
+
+        for (int ii = 0; ii < 200; ii++)
+        { // get average self-test values of gyro and acclerometer
+
+            read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 6, &raw_data[0]);           // Read the six raw data registers into data array
+            aSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value
+            aSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
+            aSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
+
+            read_bytes(mpu_i2c_addr, GYRO_XOUT_H, 6, &raw_data[0]);            // Read the six raw data registers sequentially into data array
+            gSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value
+            gSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
+            gSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
+        }
+
+        for (int ii = 0; ii < 3; ii++)
+        { // Get average of 200 values and store as average self-test readings
+            aSTAvg[ii] /= 200;
+            gSTAvg[ii] /= 200;
+        }
+
+        // Configure the gyro and accelerometer for normal operation
+        write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0x00);
+        write_byte(mpu_i2c_addr, GYRO_CONFIG, 0x00);
+        delay(25); // Delay a while to let the device stabilize
+
+        // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+        uint8_t self_test_data[6];
+        self_test_data[0] = read_byte(mpu_i2c_addr, SELF_TEST_X_ACCEL); // X-axis accel self-test results
+        self_test_data[1] = read_byte(mpu_i2c_addr, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
+        self_test_data[2] = read_byte(mpu_i2c_addr, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
+        self_test_data[3] = read_byte(mpu_i2c_addr, SELF_TEST_X_GYRO);  // X-axis gyro self-test results
+        self_test_data[4] = read_byte(mpu_i2c_addr, SELF_TEST_Y_GYRO);  // Y-axis gyro self-test results
+        self_test_data[5] = read_byte(mpu_i2c_addr, SELF_TEST_Z_GYRO);  // Z-axis gyro self-test results
+
+        // Retrieve factory self-test value from self-test code reads
+        factoryTrim[0] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[0] - 1.0))); // FT[Xa] factory trim calculation
+        factoryTrim[1] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[1] - 1.0))); // FT[Ya] factory trim calculation
+        factoryTrim[2] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[2] - 1.0))); // FT[Za] factory trim calculation
+        factoryTrim[3] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[3] - 1.0))); // FT[Xg] factory trim calculation
+        factoryTrim[4] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[4] - 1.0))); // FT[Yg] factory trim calculation
+        factoryTrim[5] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[5] - 1.0))); // FT[Zg] factory trim calculation
+
+        // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
+        // To get percent, must multiply by 100
+        for (int i = 0; i < 3; i++)
+        {
+            self_test_result[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i] - 100.;         // Report percent differences
+            self_test_result[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3] - 100.; // Report percent differences
+        }
+
+        if (b_verbose)
+        {
+            Serial.print("x-axis self test: acceleration trim within : ");
+            Serial.print(self_test_result[0], 1);
+            Serial.println("% of factory value");
+            Serial.print("y-axis self test: acceleration trim within : ");
+            Serial.print(self_test_result[1], 1);
+            Serial.println("% of factory value");
+            Serial.print("z-axis self test: acceleration trim within : ");
+            Serial.print(self_test_result[2], 1);
+            Serial.println("% of factory value");
+            Serial.print("x-axis self test: gyration trim within : ");
+            Serial.print(self_test_result[3], 1);
+            Serial.println("% of factory value");
+            Serial.print("y-axis self test: gyration trim within : ");
+            Serial.print(self_test_result[4], 1);
+            Serial.println("% of factory value");
+            Serial.print("z-axis self test: gyration trim within : ");
+            Serial.print(self_test_result[5], 1);
+            Serial.println("% of factory value");
+        }
+
+        bool b = true;
+        for (uint8_t i = 0; i < 6; ++i)
+        {
+            b &= fabs(self_test_result[i]) <= 14.f;
+        }
+        return b;
+    }
+
+    float get_acc_resolution(const ACCEL_FS_SEL accel_af_sel) const
+    {
+        switch (accel_af_sel)
+        {
+        // Possible accelerometer scales (and their register bit settings) are:
+        // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
+        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
+        case ACCEL_FS_SEL::A2G:
+            return 2.0 / 32768.0;
+        case ACCEL_FS_SEL::A4G:
+            return 4.0 / 32768.0;
+        case ACCEL_FS_SEL::A8G:
+            return 8.0 / 32768.0;
+        case ACCEL_FS_SEL::A16G:
+            return 16.0 / 32768.0;
+        default:
+            return 0.;
+        }
+    }
+
+    float get_gyro_resolution(const GYRO_FS_SEL gyro_fs_sel) const
+    {
+        switch (gyro_fs_sel)
+        {
+        // Possible gyro scales (and their register bit settings) are:
+        // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11).
+        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
+        case GYRO_FS_SEL::G250DPS:
+            return 250.0 / 32768.0;
+        case GYRO_FS_SEL::G500DPS:
+            return 500.0 / 32768.0;
+        case GYRO_FS_SEL::G1000DPS:
+            return 1000.0 / 32768.0;
+        case GYRO_FS_SEL::G2000DPS:
+            return 2000.0 / 32768.0;
+        default:
+            return 0.;
+        }
+    }
+
+    float get_mag_resolution(const MAG_OUTPUT_BITS mag_output_bits) const
+    {
+        switch (mag_output_bits)
+        {
+        // Possible magnetometer scales (and their register bit settings) are:
+        // 14 bit resolution (0) and 16 bit resolution (1)
+        // Proper scale to return milliGauss
+        case MAG_OUTPUT_BITS::M14BITS:
+            return 10. * 4912. / 8190.0;
+        case MAG_OUTPUT_BITS::M16BITS:
+            return 10. * 4912. / 32760.0;
+        default:
+            return 0.;
+        }
+    }
+
+    void write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
+    {
+        wire->beginTransmission(address);   // Initialize the Tx buffer
+        wire->write(subAddress);            // Put slave register address in Tx buffer
+        wire->write(data);                  // Put data in Tx buffer
+        i2c_err_ = wire->endTransmission(); // Send the Tx buffer
+        if (i2c_err_)
+            print_i2c_error();
+    }
+
+    uint8_t read_byte(uint8_t address, uint8_t subAddress)
+    {
+        uint8_t data = 0;                        // `data` will store the register data
+        wire->beginTransmission(address);        // Initialize the Tx buffer
+        wire->write(subAddress);                 // Put slave register address in Tx buffer
+        i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
+        if (i2c_err_)
+            print_i2c_error();
+        wire->requestFrom(address, (size_t)1); // Read one byte from slave register address
+        if (wire->available())
+            data = wire->read(); // Fill Rx buffer with result
+        return data;             // Return data read from slave register
+    }
+
+    void read_bytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
+    {
+        wire->beginTransmission(address);        // Initialize the Tx buffer
+        wire->write(subAddress);                 // Put slave register address in Tx buffer
+        i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
+        if (i2c_err_)
+            print_i2c_error();
+        uint8_t i = 0;
+        wire->requestFrom(address, count); // Read bytes from slave register address
+        while (wire->available())
+        {
+            dest[i++] = wire->read();
+        } // Put read results in the Rx buffer
+    }
+
+    void print_i2c_error()
+    {
+        if (i2c_err_ == 7)
+            return; // to avoid stickbreaker-i2c branch's error code
+        Serial.print("I2C ERROR CODE : ");
+        Serial.println(i2c_err_);
+    }
+};
+
+using MPU9250 = MPU9250_<TwoWire>;
+
+#endif // MPU9250_H
diff --git a/lib/MPU9250/README.md b/lib/MPU9250/README.md
new file mode 100644
index 0000000..96a52dc
--- /dev/null
+++ b/lib/MPU9250/README.md
@@ -0,0 +1,317 @@
+# MPU9250
+
+Arduino library for [MPU9250](https://www.invensense.com/products/motion-tracking/9-axis/mpu-9250/) Nine-Axis (Gyro + Accelerometer + Compass) MEMS MotionTracking™ Device
+
+This library is based on the [great work](https://github.com/kriswiner/MPU9250) by [kriswiner](https://github.com/kriswiner), and re-writen for the simple usage.
+
+## WARNING
+
+**MPU-9250 has been DISCONTINUED. I won't provide active support. This library supports only genuine MPU-9250, and I can't help other copy products of it**
+
+If you have a problem, first read the FAQ. After that, please search for similar issues. Please open the issue and fill out the issue template if you can't find the solution.
+
+## FAQ
+
+> There are interference between some axes
+
+There are some possibilities.
+
+- Your device may not be a genuine MPU-9250
+- Calibration is not enough
+- Gimbal lock
+
+Please refer [#62](https://github.com/hideakitai/MPU9250/issues/62), [#69](https://github.com/hideakitai/MPU9250/issues/69), etc.
+
+> Magnetometer is always zero
+
+Your device may not be a genuine MPU-9250. Please refer [#52](https://github.com/hideakitai/MPU9250/issues/52) [#72](https://github.com/hideakitai/MPU9250/issues/72)
+
+
+## Usage
+
+### Simple Measurement
+
+```C++
+#include "MPU9250.h"
+
+MPU9250 mpu; // You can also use MPU9255 as is
+
+void setup() {
+    Serial.begin(115200);
+    Wire.begin();
+    delay(2000);
+
+    mpu.setup(0x68);  // change to your own address
+}
+
+void loop() {
+    if (mpu.update()) {
+        Serial.print(mpu.getYaw()); Serial.print(", ");
+        Serial.print(mpu.getPitch()); Serial.print(", ");
+        Serial.println(mpu.getRoll());
+    }
+}
+```
+
+### Calibration
+
+- accel/gyro/mag offsets are **NOT stored** to register if the MPU has powered off ([app note](https://www.digikey.com/en/pdf/i/invensense/mpu-hardware-offset-registers))
+- need to **set all offsets at every bootup by yourself** (or calibrate at every bootup)
+- device should be stay still during accel/gyro calibration
+- round device around during mag calibration
+
+```C++
+#include "MPU9250.h"
+
+MPU9250 mpu; // You can also use MPU9255 as is
+
+void setup() {
+    Serial.begin(115200);
+    Wire.begin();
+    delay(2000);
+
+    mpu.setup(0x68);  // change to your own address
+
+    delay(5000);
+
+    // calibrate anytime you want to
+    mpu.calibrateAccelGyro();
+    mpu.calibrateMag();
+}
+
+void loop() { }
+```
+
+### Coordinate
+
+The coordinate of quaternion and roll/pitch/yaw angles are basedd on airplane coordinate (Right-Handed, X-forward, Z-down). On the other hand, the coordinate of euler angle is based on the axes of acceleration and gyro sensors (Right-Handed, X-forward, Z-up).Please use `getEulerX/Y/Z()` for euler angles and `getRoll/Pitch/Yaw()` for airplane coordinate angles.
+
+## Other Settings
+
+### I2C Address
+
+You must set your own address based on A0, A1, A2 setting as:
+
+```C++
+mpu.setup(0x70);
+```
+
+### Customize MPU9250 Configuration
+
+You can set your own setting using `MPU9250Setting` struct as:
+
+```C++
+MPU9250Setting setting;
+setting.accel_fs_sel = ACCEL_FS_SEL::A16G;
+setting.gyro_fs_sel = GYRO_FS_SEL::G2000DPS;
+setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
+setting.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_200HZ;
+setting.gyro_fchoice = 0x03;
+setting.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;
+setting.accel_fchoice = 0x01;
+setting.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_45HZ;
+
+mpu.setup(0x68, setting);
+```
+
+See `custom_setting.ino` example for detail.
+
+#### List of Settings
+
+```C++
+enum class ACCEL_FS_SEL { A2G, A4G, A8G, A16G };
+enum class GYRO_FS_SEL { G250DPS, G500DPS, G1000DPS, G2000DPS };
+enum class MAG_OUTPUT_BITS { M14BITS, M16BITS };
+
+enum class FIFO_SAMPLE_RATE : uint8_t {
+    SMPL_1000HZ,
+    SMPL_500HZ,
+    SMPL_333HZ,
+    SMPL_250HZ,
+    SMPL_200HZ,
+    SMPL_167HZ,
+    SMPL_143HZ,
+    SMPL_125HZ,
+};
+
+enum class GYRO_DLPF_CFG : uint8_t {
+    DLPF_250HZ,
+    DLPF_184HZ,
+    DLPF_92HZ,
+    DLPF_41HZ,
+    DLPF_20HZ,
+    DLPF_10HZ,
+    DLPF_5HZ,
+    DLPF_3600HZ,
+};
+
+enum class ACCEL_DLPF_CFG : uint8_t {
+    DLPF_218HZ_0,
+    DLPF_218HZ_1,
+    DLPF_99HZ,
+    DLPF_45HZ,
+    DLPF_21HZ,
+    DLPF_10HZ,
+    DLPF_5HZ,
+    DLPF_420HZ,
+};
+
+struct MPU9250Setting {
+    ACCEL_FS_SEL     accel_fs_sel {ACCEL_FS_SEL::A16G};
+    GYRO_FS_SEL      gyro_fs_sel {GYRO_FS_SEL::G2000DPS};
+    MAG_OUTPUT_BITS  mag_output_bits {MAG_OUTPUT_BITS::M16BITS};
+    FIFO_SAMPLE_RATE fifo_sample_rate {FIFO_SAMPLE_RATE::SMPL_200HZ};
+    uint8_t          gyro_fchoice {0x03};
+    GYRO_DLPF_CFG    gyro_dlpf_cfg {GYRO_DLPF_CFG::DLPF_41HZ};
+    uint8_t          accel_fchoice {0x01};
+    ACCEL_DLPF_CFG   accel_dlpf_cfg {ACCEL_DLPF_CFG::DLPF_45HZ};
+};
+```
+
+#### Magnetic Declination
+
+Magnetic declination should be set depending on where you are to get accurate data.
+To set it, use this method.
+
+```C++
+mpu.setMagneticDeclination(value);
+```
+
+You can find magnetic declination in your city [here](http://www.magnetic-declination.com/).
+
+For more details, see [wiki](https://en.wikipedia.org/wiki/Magnetic_declination).
+
+### Quaternion Filter
+
+You can choose quaternion filter using `void selectFilter(QuatFilterSel sel)`. Available quaternion filters are listed below.
+
+```C++
+enum class QuatFilterSel {
+    NONE,
+    MADGWICK, // default
+    MAHONY,
+};
+```
+
+You can also change the calculate iterations for the filter as follows. The default value is 1. Generally 10-20 is good for stable yaw estimation. Please see [this discussion](https://github.com/kriswiner/MPU9250/issues/420) for the detail.
+
+```C++
+mpu.setFilterIterations(10);
+```
+
+### Other I2C library
+
+You can use other I2C library e.g. [SoftWire](https://github.com/stevemarple/SoftWire).
+
+```C++
+MPU9250_<SoftWire> mpu;
+SoftWire sw(SDA, SCL);
+
+// you need setting struct
+MPU9250Setting setting;
+
+// in setup()
+mpu.setup(0x70, setting, sw);
+```
+
+## About I2C Errors
+
+Sometimes this library shows the I2C error number if your connection is not correct. It's based on the I2C error number which is reported by the `Wire.endTransmission()`. It returns following number based on the result of I2C data transmission.
+
+> 0:success
+> 1:data too long to fit in transmit buffer
+> 2:received NACK on transmit of address
+> 3:received NACK on transmit of data
+> 4:other error
+
+If you have such errors, please check your hardware connection and I2C address setting first. Please refer [Wire.endTransmission() reference](https://www.arduino.cc/en/Reference/WireEndTransmission) for these errors, and [section 2.3 of this explanation](https://www.ti.com/lit/an/slva704/slva704.pdf) for ACK and NACK.
+
+## APIs
+
+```C++
+bool setup(const uint8_t addr, const MPU9250Setting& setting, WireType& w = Wire);
+void verbose(const bool b);
+void ahrs(const bool b);
+void sleep(bool b);
+void calibrateAccelGyro();
+void calibrateMag();
+bool isConnected();
+bool isConnectedMPU9250();
+bool isConnectedAK8963();
+bool isSleeping();
+bool available();
+bool update();
+void update_accel_gyro();
+void update_mag();
+void update_rpy(float qw, float qx, float qy, float qz);
+
+float getRoll() const;
+float getPitch() const;
+float getYaw() const;
+
+float getEulerX() const;
+float getEulerY() const;
+float getEulerZ() const;
+
+float getQuaternionX() const;
+float getQuaternionY() const;
+float getQuaternionZ() const;
+float getQuaternionW() const;
+
+float getAcc(const uint8_t i) const;
+float getGyro(const uint8_t i) const;
+float getMag(const uint8_t i) const;
+float getLinearAcc(const uint8_t i) const;
+
+float getAccX() const;
+float getAccY() const;
+float getAccZ() const;
+float getGyroX() const;
+float getGyroY() const;
+float getGyroZ() const;
+float getMagX() const;
+float getMagY() const;
+float getMagZ() const;
+float getLinearAccX() const;
+float getLinearAccY() const;
+float getLinearAccZ() const;
+
+float getAccBias(const uint8_t i) const;
+float getGyroBias(const uint8_t i) const;
+float getMagBias(const uint8_t i) const;
+float getMagScale(const uint8_t i) const;
+
+float getAccBiasX() const;
+float getAccBiasY() const;
+float getAccBiasZ() const;
+float getGyroBiasX() const;
+float getGyroBiasY() const;
+float getGyroBiasZ() const;
+float getMagBiasX() const;
+float getMagBiasY() const;
+float getMagBiasZ() const;
+float getMagScaleX() const;
+float getMagScaleY() const;
+float getMagScaleZ() const;
+
+float getTemperature() const;
+
+void setAccBias(const float x, const float y, const float z);
+void setGyroBias(const float x, const float y, const float z);
+void setMagBias(const float x, const float y, const float z);
+void setMagScale(const float x, const float y, const float z);
+void setMagneticDeclination(const float d);
+
+void selectFilter(QuatFilterSel sel);
+void setFilterIterations(const size_t n);
+
+bool selftest();
+```
+
+## License
+
+MIT
+
+## Acknowledgments / Contributor
+
+- [Yuta Asai](https://github.com/asaiyuta)
diff --git a/lib/MPU9250/library.json b/lib/MPU9250/library.json
new file mode 100644
index 0000000..6563f52
--- /dev/null
+++ b/lib/MPU9250/library.json
@@ -0,0 +1,20 @@
+{
+    "name": "MPU9250",
+    "keywords": "i2c,wire,imu",
+    "description": "Arduino library for MPU9250 Nine-Axis (Gyro + Accelerometer + Compass) MEMS MotionTracking™ Device",
+    "repository":
+    {
+        "type": "git",
+        "url": "https://github.com/hideakitai/MPU9250.git"
+    },
+    "authors":
+    {
+        "name": "Hideaki Tai",
+        "url": "https://github.com/hideakitai",
+        "maintainer": true
+    },
+    "version": "0.4.8",
+    "license": "MIT",
+    "frameworks": "arduino",
+    "platforms": "*"
+}
diff --git a/lib/MPU9250/library.properties b/lib/MPU9250/library.properties
new file mode 100644
index 0000000..96d0928
--- /dev/null
+++ b/lib/MPU9250/library.properties
@@ -0,0 +1,9 @@
+name=MPU9250
+version=0.4.8
+author=hideakitai
+maintainer=hideakitai
+sentence=Arduino library for MPU9250 Nine-Axis (Gyro + Accelerometer + Compass) MEMS MotionTracking™ Device
+paragraph=Arduino library for MPU9250 Nine-Axis (Gyro + Accelerometer + Compass) MEMS MotionTracking™ Device
+category=Device Control
+url=https://github.com/hideakitai/MPU9250
+architectures=*
diff --git a/lib/MPU9250/mpu9250/MPU9250RegisterMap.h b/lib/MPU9250/mpu9250/MPU9250RegisterMap.h
new file mode 100644
index 0000000..0029198
--- /dev/null
+++ b/lib/MPU9250/mpu9250/MPU9250RegisterMap.h
@@ -0,0 +1,151 @@
+#pragma once
+#ifndef MPU9250REGISTERMAP_H
+#define MPU9250REGISTERMAP_H
+
+//Magnetometer Registers
+// #define AK8963_ADDRESS   0x0C
+#define AK8963_WHO_AM_I  0x00 // should return 0x48
+#define AK8963_INFO      0x01
+#define AK8963_ST1       0x02  // data ready status bit 0
+#define AK8963_XOUT_L	 0x03  // data
+#define AK8963_XOUT_H	 0x04
+#define AK8963_YOUT_L	 0x05
+#define AK8963_YOUT_H	 0x06
+#define AK8963_ZOUT_L	 0x07
+#define AK8963_ZOUT_H	 0x08
+#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC      0x0C  // Self test control
+#define AK8963_I2CDIS    0x0F  // I2C disable
+#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value
+
+#define SELF_TEST_X_GYRO 0x00
+#define SELF_TEST_Y_GYRO 0x01
+#define SELF_TEST_Z_GYRO 0x02
+
+// #define X_FINE_GAIN      0x03 // [7:0] fine gain
+// #define Y_FINE_GAIN      0x04
+// #define Z_FINE_GAIN      0x05
+// #define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
+// #define XA_OFFSET_L_TC   0x07
+// #define YA_OFFSET_H      0x08
+// #define YA_OFFSET_L_TC   0x09
+// #define ZA_OFFSET_H      0x0A
+// #define ZA_OFFSET_L_TC   0x0B
+
+#define SELF_TEST_X_ACCEL 0x0D
+#define SELF_TEST_Y_ACCEL 0x0E
+#define SELF_TEST_Z_ACCEL 0x0F
+
+#define SELF_TEST_A      0x10
+
+#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope
+#define XG_OFFSET_L      0x14
+#define YG_OFFSET_H      0x15
+#define YG_OFFSET_L      0x16
+#define ZG_OFFSET_H      0x17
+#define ZG_OFFSET_L      0x18
+#define SMPLRT_DIV       0x19
+#define MPU_CONFIG           0x1A
+#define GYRO_CONFIG      0x1B
+#define ACCEL_CONFIG     0x1C
+#define ACCEL_CONFIG2    0x1D
+#define LP_ACCEL_ODR     0x1E
+#define WOM_THR          0x1F
+
+#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+
+#define FIFO_EN          0x23
+#define I2C_MST_CTRL     0x24
+#define I2C_SLV0_ADDR    0x25
+#define I2C_SLV0_REG     0x26
+#define I2C_SLV0_CTRL    0x27
+#define I2C_SLV1_ADDR    0x28
+#define I2C_SLV1_REG     0x29
+#define I2C_SLV1_CTRL    0x2A
+#define I2C_SLV2_ADDR    0x2B
+#define I2C_SLV2_REG     0x2C
+#define I2C_SLV2_CTRL    0x2D
+#define I2C_SLV3_ADDR    0x2E
+#define I2C_SLV3_REG     0x2F
+#define I2C_SLV3_CTRL    0x30
+#define I2C_SLV4_ADDR    0x31
+#define I2C_SLV4_REG     0x32
+#define I2C_SLV4_DO      0x33
+#define I2C_SLV4_CTRL    0x34
+#define I2C_SLV4_DI      0x35
+#define I2C_MST_STATUS   0x36
+#define INT_PIN_CFG      0x37
+#define INT_ENABLE       0x38
+#define DMP_INT_STATUS   0x39  // Check DMP interrupt
+#define INT_STATUS       0x3A
+#define ACCEL_XOUT_H     0x3B
+#define ACCEL_XOUT_L     0x3C
+#define ACCEL_YOUT_H     0x3D
+#define ACCEL_YOUT_L     0x3E
+#define ACCEL_ZOUT_H     0x3F
+#define ACCEL_ZOUT_L     0x40
+#define TEMP_OUT_H       0x41
+#define TEMP_OUT_L       0x42
+#define GYRO_XOUT_H      0x43
+#define GYRO_XOUT_L      0x44
+#define GYRO_YOUT_H      0x45
+#define GYRO_YOUT_L      0x46
+#define GYRO_ZOUT_H      0x47
+#define GYRO_ZOUT_L      0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO      0x63
+#define I2C_SLV1_DO      0x64
+#define I2C_SLV2_DO      0x65
+#define I2C_SLV3_DO      0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET  0x68
+#define MOT_DETECT_CTRL  0x69
+#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2       0x6C
+#define DMP_BANK         0x6D  // Activates a specific bank in the DMP
+#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
+#define DMP_REG_1        0x70
+#define DMP_REG_2        0x71
+#define FIFO_COUNTH      0x72
+#define FIFO_COUNTL      0x73
+#define FIFO_R_W         0x74
+#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
+#define XA_OFFSET_H      0x77
+#define XA_OFFSET_L      0x78
+#define YA_OFFSET_H      0x7A
+#define YA_OFFSET_L      0x7B
+#define ZA_OFFSET_H      0x7D
+#define ZA_OFFSET_L      0x7E
+
+#endif // MPU9250REGISTERMAP_H
diff --git a/lib/MPU9250/mpu9250/QuaternionFilter.h b/lib/MPU9250/mpu9250/QuaternionFilter.h
new file mode 100644
index 0000000..42b600c
--- /dev/null
+++ b/lib/MPU9250/mpu9250/QuaternionFilter.h
@@ -0,0 +1,237 @@
+#pragma once
+#ifndef QUATERNIONFILTER_H
+#define QUATERNIONFILTER_H
+
+enum class QuatFilterSel {
+    NONE,
+    MADGWICK,
+    MAHONY,
+};
+
+class QuaternionFilter {
+    // for madgwick
+    float GyroMeasError = PI * (40.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 40 deg/s)
+    float GyroMeasDrift = PI * (0.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+    float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
+    float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+
+    // for mahony
+    float Kp = 30.0;
+    float Ki = 0.0;
+
+    QuatFilterSel filter_sel{QuatFilterSel::MADGWICK};
+    double deltaT{0.};
+    uint32_t newTime{0}, oldTime{0};
+
+public:
+    void select_filter(QuatFilterSel sel) {
+        filter_sel = sel;
+    }
+
+    void update(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
+        newTime = micros();
+        deltaT = newTime - oldTime;
+        oldTime = newTime;
+        deltaT = fabs(deltaT * 0.001 * 0.001);
+
+        switch (filter_sel) {
+            case QuatFilterSel::MADGWICK:
+                madgwick(ax, ay, az, gx, gy, gz, mx, my, mz, q);
+                break;
+            case QuatFilterSel::MAHONY:
+                mahony(ax, ay, az, gx, gy, gz, mx, my, mz, q);
+                break;
+            default:
+                no_filter(ax, ay, az, gx, gy, gz, mx, my, mz, q);
+                break;
+        }
+    }
+
+    void no_filter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
+        float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];  // variable for readability
+        q[0] += 0.5f * (-q1 * gx - q2 * gy - q3 * gz) * deltaT;
+        q[1] += 0.5f * (q0 * gx + q2 * gz - q3 * gy) * deltaT;
+        q[2] += 0.5f * (q0 * gy - q1 * gz + q3 * gx) * deltaT;
+        q[3] += 0.5f * (q0 * gz + q1 * gy - q2 * gx) * deltaT;
+        float recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+        q[0] *= recipNorm;
+        q[1] *= recipNorm;
+        q[2] *= recipNorm;
+        q[3] *= recipNorm;
+    }
+
+    // Madgwick Quaternion Update
+    void madgwick(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
+        double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];  // short name local variable for readability
+        double recipNorm;
+        double s0, s1, s2, s3;
+        double qDot1, qDot2, qDot3, qDot4;
+        double hx, hy;
+        double _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+        // Rate of change of quaternion from gyroscope
+        qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+        qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+        qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+        qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+        // Normalise accelerometer measurement
+        double a_norm = ax * ax + ay * ay + az * az;
+        if (a_norm == 0.) return;  // handle NaN
+        recipNorm = 1.0 / sqrt(a_norm);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;
+
+        // Normalise magnetometer measurement
+        double m_norm = mx * mx + my * my + mz * mz;
+        if (m_norm == 0.) return;  // handle NaN
+        recipNorm = 1.0 / sqrt(m_norm);
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0mx = 2.0f * q0 * mx;
+        _2q0my = 2.0f * q0 * my;
+        _2q0mz = 2.0f * q0 * mz;
+        _2q1mx = 2.0f * q1 * mx;
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _2q0q2 = 2.0f * q0 * q2;
+        _2q2q3 = 2.0f * q2 * q3;
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);  // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+
+        // Integrate rate of change of quaternion to yield quaternion
+        q0 += qDot1 * deltaT;
+        q1 += qDot2 * deltaT;
+        q2 += qDot3 * deltaT;
+        q3 += qDot4 * deltaT;
+
+        // Normalise quaternion
+        recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+        q0 *= recipNorm;
+        q1 *= recipNorm;
+        q2 *= recipNorm;
+        q3 *= recipNorm;
+
+        q[0] = q0;
+        q[1] = q1;
+        q[2] = q2;
+        q[3] = q3;
+    }
+
+    // Mahony accelleration filter
+    // Mahony scheme uses proportional and integral filtering on
+    // the error between estimated reference vector (gravity) and measured one.
+    // Madgwick's implementation of Mayhony's AHRS algorithm.
+    // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+    // Free parameters in the Mahony filter and fusion scheme,
+    // Kp for proportional feedback, Ki for integral
+    // float Kp = 30.0;
+    // float Ki = 0.0;
+    // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
+    // with MPU-6050, some instability observed at Kp=100 Now set to 30.
+    void mahony(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
+        float recipNorm;
+        float vx, vy, vz;
+        float ex, ey, ez;  //error terms
+        float qa, qb, qc;
+        static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
+        float tmp;
+
+        // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+        tmp = ax * ax + ay * ay + az * az;
+        if (tmp > 0.0) {
+            // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
+            recipNorm = 1.0 / sqrt(tmp);
+            ax *= recipNorm;
+            ay *= recipNorm;
+            az *= recipNorm;
+
+            // Estimated direction of gravity in the body frame (factor of two divided out)
+            vx = q[1] * q[3] - q[0] * q[2];
+            vy = q[0] * q[1] + q[2] * q[3];
+            vz = q[0] * q[0] - 0.5f + q[3] * q[3];
+
+            // Error is cross product between estimated and measured direction of gravity in body frame
+            // (half the actual magnitude)
+            ex = (ay * vz - az * vy);
+            ey = (az * vx - ax * vz);
+            ez = (ax * vy - ay * vx);
+
+            // Compute and apply to gyro term the integral feedback, if enabled
+            if (Ki > 0.0f) {
+                ix += Ki * ex * deltaT;  // integral error scaled by Ki
+                iy += Ki * ey * deltaT;
+                iz += Ki * ez * deltaT;
+                gx += ix;  // apply integral feedback
+                gy += iy;
+                gz += iz;
+            }
+
+            // Apply proportional feedback to gyro term
+            gx += Kp * ex;
+            gy += Kp * ey;
+            gz += Kp * ez;
+        }
+
+        // Integrate rate of change of quaternion, q cross gyro term
+        deltaT = 0.5 * deltaT;
+        gx *= deltaT;  // pre-multiply common factors
+        gy *= deltaT;
+        gz *= deltaT;
+        qa = q[0];
+        qb = q[1];
+        qc = q[2];
+        q[0] += (-qb * gx - qc * gy - q[3] * gz);
+        q[1] += (qa * gx + qc * gz - q[3] * gy);
+        q[2] += (qa * gy - qb * gz + q[3] * gx);
+        q[3] += (qa * gz + qb * gy - qc * gx);
+
+        // renormalise quaternion
+        recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+        q[0] = q[0] * recipNorm;
+        q[1] = q[1] * recipNorm;
+        q[2] = q[2] * recipNorm;
+        q[3] = q[3] * recipNorm;
+    }
+};
+
+#endif  // QUATERNIONFILTER_H
diff --git a/platformio.ini b/platformio.ini
index 41f331b..fa20942 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -1,4 +1,4 @@
-;PlatformIO Project Configuration File
+; PlatformIO Project Configuration File
 ;
 ;   Build options: build flags, source filter
 ;   Upload options: custom upload port, speed and extra flags
@@ -12,11 +12,4 @@
 platform = espressif32@2.1.0
 board = nodemcu-32s
 framework = arduino
-
-; Serial Monitor options
 monitor_speed = 115200
-
-; Automatic static checks for errors
-; https://docs.platformio.org/en/latest/plus/check-tools/cppcheck.html
-; check_tool = cppcheck
-; check_flags = --enable=all
diff --git a/src/config/pins.h b/src/config/pins.h
index 2875fc1..272d4b0 100644
--- a/src/config/pins.h
+++ b/src/config/pins.h
@@ -28,3 +28,4 @@
 // ------------------------------------------------------------------ Other Pins
 
 // --------------------------------------------------------- I2C  Port addresses
+#define I2C_ADDRESS_MPU9250 0x68
\ No newline at end of file
diff --git a/src/define.h b/src/define.h
index b523fad..f787c4d 100644
--- a/src/define.h
+++ b/src/define.h
@@ -19,5 +19,8 @@
 
 // *** Modules **************************************************
 
+// Motion Sensor
+#include "sensors/motion/motion.h"
+
 // Motors
-#include "modules/motors/motors.h"
\ No newline at end of file
+#include "modules/motors/motors.h"
diff --git a/src/sensors/motion/motion.cpp b/src/sensors/motion/motion.cpp
new file mode 100644
index 0000000..d55f3a6
--- /dev/null
+++ b/src/sensors/motion/motion.cpp
@@ -0,0 +1,3 @@
+#include "motion.h"
+
+SW_MPU9250 motion(false);
\ No newline at end of file
diff --git a/src/sensors/motion/motion.h b/src/sensors/motion/motion.h
new file mode 100644
index 0000000..2e01650
--- /dev/null
+++ b/src/sensors/motion/motion.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include "./src/robot_mpu9250.h"
+
+extern SW_MPU9250 motion;
\ No newline at end of file
diff --git a/src/sensors/motion/src/robot_mpu9250.cpp b/src/sensors/motion/src/robot_mpu9250.cpp
new file mode 100644
index 0000000..fc5b6ef
--- /dev/null
+++ b/src/sensors/motion/src/robot_mpu9250.cpp
@@ -0,0 +1,101 @@
+#include <Wire.h>
+
+#include "robot_mpu9250.h"
+#include "config/pins.h"
+#include "MPU9250.h"
+
+MPU9250 mpu;
+
+SW_MPU9250::SW_MPU9250(bool useCalibration)
+{
+    // TODO: useCalibration config
+}
+
+void SW_MPU9250::begin()
+{
+    Wire.begin();
+    delay(2000);
+    if (!mpu.setup(I2C_ADDRESS_MPU9250))
+    { // change to your own address
+        while (1)
+        {
+            Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
+            delay(5000);
+        }
+    }
+
+    Serial.println(">> Motion\t:enabled");
+}
+
+void SW_MPU9250::calibrate()
+{
+    // calibrate anytime you want to
+    Serial.println("Accel Gyro calibration will start in 5sec.");
+    Serial.println("Please leave the device still on the flat plane.");
+    mpu.verbose(true);
+    delay(5000);
+    mpu.calibrateAccelGyro();
+
+    Serial.println("Mag calibration will start in 5sec.");
+    Serial.println("Please Wave device in a figure eight until done.");
+    delay(5000);
+    mpu.calibrateMag();
+
+    print_calibration();
+    mpu.verbose(false);
+}
+
+void SW_MPU9250::read()
+{
+}
+
+void SW_MPU9250::test()
+{
+    if (mpu.update())
+    {
+        static uint32_t prev_ms = millis();
+        if (millis() > prev_ms + 25)
+        {
+            print_roll_pitch_yaw();
+            prev_ms = millis();
+        }
+    }
+}
+
+void SW_MPU9250::print_calibration()
+{
+    Serial.println("< calibration parameters >");
+    Serial.println("accel bias [g]: ");
+    Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.println();
+    Serial.println("gyro bias [deg/s]: ");
+    Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.println();
+    Serial.println("mag bias [mG]: ");
+    Serial.print(mpu.getMagBiasX());
+    Serial.print(", ");
+    Serial.print(mpu.getMagBiasY());
+    Serial.print(", ");
+    Serial.print(mpu.getMagBiasZ());
+    Serial.println();
+    Serial.println("mag scale []: ");
+    Serial.print(mpu.getMagScaleX());
+    Serial.print(", ");
+    Serial.print(mpu.getMagScaleY());
+    Serial.print(", ");
+    Serial.print(mpu.getMagScaleZ());
+    Serial.println();
+}
+
+void SW_MPU9250::print_roll_pitch_yaw()
+{
+    Serial.printf("%8.2f %8.2f %8.2f\n", mpu.getYaw(), mpu.getPitch(), mpu.getRoll());
+}
\ No newline at end of file
diff --git a/src/sensors/motion/src/robot_mpu9250.h b/src/sensors/motion/src/robot_mpu9250.h
new file mode 100644
index 0000000..9bf4dcc
--- /dev/null
+++ b/src/sensors/motion/src/robot_mpu9250.h
@@ -0,0 +1,24 @@
+#ifndef MPU9250_h
+#define MPU9250_h
+
+#include <Arduino.h>
+
+class SW_MPU9250
+{
+public:
+    SW_MPU9250(bool useCalibration);
+
+    void begin();
+
+    // TODO
+    void read();
+
+    void calibrate();
+    void test();
+
+private:
+    void print_calibration();
+    void print_roll_pitch_yaw();
+};
+
+#endif
\ No newline at end of file
diff --git a/src/src.cpp b/src/src.cpp
index 6f49ff8..378366e 100644
--- a/src/src.cpp
+++ b/src/src.cpp
@@ -10,34 +10,20 @@ int ROBOT_ID;
 
 void setup()
 {
-    // put your setup code here, to run once:
-
-    // Enables Serial Communication with baudRate of 115200
     Serial.begin(115200);
-    Serial.println("PeraSwarm Robot v5");
-
-    memory.begin(); // NOTE: This should be run as the first thing.
-
-    // This commands should be run 'ONLY' at the first run to assign a ID for robot
-    // memory.setupRobotWithId(RobotId)
-    // memory.setupRobotWithId(0);
-    ROBOT_ID = memory.getRobotId();
+    motion.begin();
+    
+    // motion.calibrate();
+    
+    memory.begin();
+    // memory.saveCalibration();
+    memory.loadCalibration();
 
-    pinMode(PIN_LED_INBUILT, OUTPUT);
-
-    // Scan available I2C devices
-    i2c_scan();
-
-    motors.begin();
 }
 
 void loop()
 {
-    // put your main code here, to run repeatedly:
-
-    motors.write(100, 100);
-    delay(1000);
-    motors.encoder_print();
+    motion.test();
 }
 
 #endif
\ No newline at end of file
diff --git a/src/utilities/memory/src/robot_memory.cpp b/src/utilities/memory/src/robot_memory.cpp
index 178575a..aaf442a 100644
--- a/src/utilities/memory/src/robot_memory.cpp
+++ b/src/utilities/memory/src/robot_memory.cpp
@@ -14,10 +14,172 @@
 #include "config/pins.h"
 #include "config/definitions.h"
 
+#include "MPU9250.h"
+const uint8_t EEPROM_SIZE = 1 + sizeof(float) * 3 * 4 + 10;
+extern MPU9250 mpu;
+
+
 #define MIN_ROBOT_ID 0
 #define MAX_ROBOT_ID 31
+// #define EEPROM_SIZE 10
+
+
+
+
+
+
+enum EEP_ADDR {
+    EEP_CALIB_FLAG = 0x00,
+    EEP_ACC_BIAS = 0x01,
+    EEP_GYRO_BIAS = 0x0D,
+    EEP_MAG_BIAS = 0x19,
+    EEP_MAG_SCALE = 0x25
+};
+
+void SW_Memory::writeByte(int address, byte value) {
+    EEPROM.put(address, value);
+}
+
+void SW_Memory::writeFloat(int address, float value) {
+    EEPROM.put(address, value);
+}
+
+byte SW_Memory::readByte(int address) {
+    byte valueIn = 0;
+    EEPROM.get(address, valueIn);
+    return valueIn;
+}
+
+float SW_Memory::readFloat(int address) {
+    float valueIn = 0;
+    EEPROM.get(address, valueIn);
+    return valueIn;
+}
+
+void SW_Memory::clearCalibration() {
+    writeByte(EEP_CALIB_FLAG, 0);
+}
+
+bool SW_Memory::isCalibrated() {
+    return (readByte(EEP_CALIB_FLAG) == 0x01);
+}
+
+void SW_Memory::saveCalibration() {
+    Serial.println("Write calibrated parameters to EEPROM");
+    writeByte(EEP_CALIB_FLAG, 1);
+    writeFloat(EEP_ACC_BIAS + 0, mpu.getAccBias(0));
+    writeFloat(EEP_ACC_BIAS + 4, mpu.getAccBias(1));
+    writeFloat(EEP_ACC_BIAS + 8, mpu.getAccBias(2));
+    writeFloat(EEP_GYRO_BIAS + 0, mpu.getGyroBias(0));
+    writeFloat(EEP_GYRO_BIAS + 4, mpu.getGyroBias(1));
+    writeFloat(EEP_GYRO_BIAS + 8, mpu.getGyroBias(2));
+    writeFloat(EEP_MAG_BIAS + 0, mpu.getMagBias(0));
+    writeFloat(EEP_MAG_BIAS + 4, mpu.getMagBias(1));
+    writeFloat(EEP_MAG_BIAS + 8, mpu.getMagBias(2));
+    writeFloat(EEP_MAG_SCALE + 0, mpu.getMagScale(0));
+    writeFloat(EEP_MAG_SCALE + 4, mpu.getMagScale(1));
+    writeFloat(EEP_MAG_SCALE + 8, mpu.getMagScale(2));
+#if defined(ESP_PLATFORM) || defined(ESP8266)
+    EEPROM.commit();
+#endif
+}
+
+void SW_Memory::loadCalibration() {
+    Serial.println("Load calibrated parameters from EEPROM");
+    if (isCalibrated()) {
+        Serial.println("calibrated? : YES");
+        Serial.println("load calibrated values");
+        mpu.setAccBias(
+            readFloat(EEP_ACC_BIAS + 0),
+            readFloat(EEP_ACC_BIAS + 4),
+            readFloat(EEP_ACC_BIAS + 8));
+        mpu.setGyroBias(
+            readFloat(EEP_GYRO_BIAS + 0),
+            readFloat(EEP_GYRO_BIAS + 4),
+            readFloat(EEP_GYRO_BIAS + 8));
+        mpu.setMagBias(
+            readFloat(EEP_MAG_BIAS + 0),
+            readFloat(EEP_MAG_BIAS + 4),
+            readFloat(EEP_MAG_BIAS + 8));
+        mpu.setMagScale(
+            readFloat(EEP_MAG_SCALE + 0),
+            readFloat(EEP_MAG_SCALE + 4),
+            readFloat(EEP_MAG_SCALE + 8));
+    } else {
+        Serial.println("calibrated? : NO");
+        Serial.println("load default values");
+        mpu.setAccBias(0., 0., 0.);
+        mpu.setGyroBias(0., 0., 0.);
+        mpu.setMagBias(0., 0., 0.);
+        mpu.setMagScale(1., 1., 1.);
+    }
+}
+
+void SW_Memory::printCalibration() {
+    Serial.println("< calibration parameters >");
+    Serial.print("calibrated? : ");
+    Serial.println(readByte(EEP_CALIB_FLAG) ? "YES" : "NO");
+    Serial.print("acc bias x  : ");
+    Serial.println(readFloat(EEP_ACC_BIAS + 0) * 1000.f / MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print("acc bias y  : ");
+    Serial.println(readFloat(EEP_ACC_BIAS + 4) * 1000.f / MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print("acc bias z  : ");
+    Serial.println(readFloat(EEP_ACC_BIAS + 8) * 1000.f / MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print("gyro bias x : ");
+    Serial.println(readFloat(EEP_GYRO_BIAS + 0) / MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print("gyro bias y : ");
+    Serial.println(readFloat(EEP_GYRO_BIAS + 4) / MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print("gyro bias z : ");
+    Serial.println(readFloat(EEP_GYRO_BIAS + 8) / MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print("mag bias x  : ");
+    Serial.println(readFloat(EEP_MAG_BIAS + 0));
+    Serial.print("mag bias y  : ");
+    Serial.println(readFloat(EEP_MAG_BIAS + 4));
+    Serial.print("mag bias z  : ");
+    Serial.println(readFloat(EEP_MAG_BIAS + 8));
+    Serial.print("mag scale x : ");
+    Serial.println(readFloat(EEP_MAG_SCALE + 0));
+    Serial.print("mag scale y : ");
+    Serial.println(readFloat(EEP_MAG_SCALE + 4));
+    Serial.print("mag scale z : ");
+    Serial.println(readFloat(EEP_MAG_SCALE + 8));
+}
+
+void SW_Memory::printBytes() {
+    for (size_t i = 0; i < EEPROM_SIZE; ++i)
+        Serial.println(readByte(i), HEX);
+}
+
+void SW_Memory::setupEEPROM() {
+    Serial.println("EEPROM start");
+
+    if (!isCalibrated()) {
+        Serial.println("Need Calibration!!");
+    }
+    Serial.println("EEPROM calibration value is : ");
+    printCalibration();
+    Serial.println("Loaded calibration value is : ");
+    loadCalibration();
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
-#define EEPROM_SIZE 10
 
 SW_Memory::SW_Memory()
 { // Memory constructor
diff --git a/src/utilities/memory/src/robot_memory.h b/src/utilities/memory/src/robot_memory.h
index b3d0b92..44c9a04 100644
--- a/src/utilities/memory/src/robot_memory.h
+++ b/src/utilities/memory/src/robot_memory.h
@@ -15,6 +15,18 @@
 class SW_Memory
 {
 public:
+    void writeByte(int address, byte value);
+    void writeFloat(int address, float value);
+    byte readByte(int address);
+    float readFloat(int address);
+    void clearCalibration();
+    bool isCalibrated();
+    void saveCalibration();
+    void loadCalibration();
+    void printCalibration();
+    void printBytes();
+    void setupEEPROM();
+
     SW_Memory();
 
     int getRobotId();