diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml
index a0af7c2..71cc361 100644
--- a/.github/workflows/build-test.yml
+++ b/.github/workflows/build-test.yml
@@ -8,7 +8,18 @@ jobs:
     strategy:
       matrix:
         example:
-          - examples/motors
+          - battery
+          - buzzer
+          - leds
+          - motorDriveToPosition
+          - motorPower
+          - motors
+          - mpu
+          - oled
+          - readEncoderPosition
+          - stupidServos
+          - ultrasoundBasic
+          - ultrasoundLeds
     steps:
       - uses: actions/checkout@v1
       - name: Set up Python
@@ -20,4 +31,4 @@ jobs:
       - name: Build examples
         run: platformio ci --lib="." --project-conf="./platformio.ini"
         env:
-          PLATFORMIO_CI_SRC: ${{ matrix.example }}
+          PLATFORMIO_CI_SRC: examples/${{ matrix.example }}
diff --git a/examples/mpu/main.cpp b/examples/mpu/main.cpp
index 35b851e..7f28f11 100644
--- a/examples/mpu/main.cpp
+++ b/examples/mpu/main.cpp
@@ -26,13 +26,19 @@ void setup() {
     auto& mpu = rb::Manager::get().mpu();
 
     mpu.init();
+
+    // sets the 0 positions to current RBCX orientation
+    mpu.calibrateNow();
+    delay(1000);
+
     mpu.sendStart();
 
     while (true) {
-        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
+        auto angle = mpu.getAngle();
+        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", angle.x, angle.y, angle.z);
         delay(100);
     }
 
 }
 
-void loop() {}
\ No newline at end of file
+void loop() {}
diff --git a/examples/mpu_oled/main.cpp b/examples/mpu_oled/main.cpp
new file mode 100644
index 0000000..e1540e3
--- /dev/null
+++ b/examples/mpu_oled/main.cpp
@@ -0,0 +1,74 @@
+#include <Arduino.h>
+#include "RBCX.h"
+
+#define D_WIDTH 128
+#define D_HEIGHT 64
+
+/**
+ * @brief Show ball in the center of the screen and move it with y and z axis
+ * @param oled OLED display
+ * @param angle MPU angle
+*/
+void showCenterGyroBall(rb::Oled& oled, rb::Mpu &mpu) {
+    // show ball in the ceter of the screen - move with x an y axis
+    auto angle = mpu.getAngle();
+    oled.fill(rb::Oled::Black);
+    int x = D_WIDTH / 2 + angle.z * 2;
+    int y = D_HEIGHT / 2 + angle.y * 2;
+    oled.drawCircle(x, y, 5, rb::Oled::White);
+    oled.updateScreen();
+}
+
+/**
+ * @brief Show gyro info on the screen
+ * @param oled OLED display
+ * @param angle MPU angle
+*/
+void showGyroIInfo(rb::Oled& oled, rb::Mpu &mpu) {
+    auto angle = mpu.getAngle();
+    oled.fill(rb::Oled::Black);
+    oled.setCursor(0, 0);
+    oled.writeString(String(angle.x).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.setCursor(0, 20);
+    oled.writeString(String(angle.y).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.setCursor(0, 40);
+    oled.writeString(String(angle.z).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.updateScreen();
+}
+
+void setup() {
+    printf("RB3204-RBCX\n");
+
+    delay(500);
+
+    printf("Init manager\n");
+    auto& man = rb::Manager::get();
+    man.install();
+
+    man.leds().blue(true);
+
+    auto &oled = rb::Manager::get().oled();
+    auto &mpu = rb::Manager::get().mpu();
+    mpu.init();
+
+    mpu.calibrateNow();
+    mpu.sendStart();
+
+    oled.init(rb::Oled::Oled_128x64, true, false);
+
+    while (true) {
+        auto angle = mpu.getAngle();
+        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", angle.x, angle.y, angle.z);
+
+        // every 3 swithc between show ball and show info
+        if (millis() % 10000 < 5000) {
+            showCenterGyroBall(oled, mpu);
+        } else {
+            showGyroIInfo(oled, mpu);
+        }
+
+        delay(50);
+    }
+}
+
+void loop() {}
\ No newline at end of file
diff --git a/examples/oled/main.cpp b/examples/oled/main.cpp
index 278d5d1..73dfc7b 100644
--- a/examples/oled/main.cpp
+++ b/examples/oled/main.cpp
@@ -56,7 +56,7 @@ void setup() {
         
         oled.setCursor(5, oled.getHeight()/2);
         String text = "OLED w:" + String(oled.getWidth()) + " | h:" + String(oled.getHeight());
-        oled.writeString(text, rb::Oled::Font_7x10, rb::Oled::White);
+        oled.writeString(text.c_str(), rb::Oled::Font_7x10, rb::Oled::White);
         oled.updateScreen();        
         waitToNextTest();
 
@@ -95,4 +95,4 @@ void setup() {
     }
 }
 
-void loop() {}
\ No newline at end of file
+void loop() {}
diff --git a/library.json b/library.json
index 4c9162c..d576a68 100644
--- a/library.json
+++ b/library.json
@@ -28,14 +28,14 @@
   "dependencies": [
     {
       "name": "RB3204-RBCX-coproc-comm",
-      "version": "https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/1ed2bbc8fda0b9b1c6592f5f39bd9bbb1213d569.zip"
+      "version": "https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/77f4ada13fb3b348cca408ac8f614597e61c370c.zip"
     },
     {
       "name": "Esp32-lx16a",
       "version": "https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip"
     }
   ],
-  "version": "1.5.0",
+  "version": "1.6.0",
   "frameworks": ["espidf", "arduino"],
   "platforms": "espressif32",
   "build": {
diff --git a/platformio.ini b/platformio.ini
index 72f5ee4..d35dbb4 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -23,6 +23,6 @@ build_flags =
     -fmax-errors=5
 
 lib_deps =
-    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/1ed2bbc8fda0b9b1c6592f5f39bd9bbb1213d569.zip
+    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/77f4ada13fb3b348cca408ac8f614597e61c370c.zip
     https://github.com/nanopb/nanopb/archive/nanopb-0.4.3.zip
     https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip
diff --git a/src/RBCXManager.cpp b/src/RBCXManager.cpp
index ce151b3..af9c5c0 100644
--- a/src/RBCXManager.cpp
+++ b/src/RBCXManager.cpp
@@ -158,6 +158,9 @@ void Manager::consumerRoutine() {
         case CoprocStat_mpuStat_tag:
             m_mpu.setState(msg.payload.mpuStat);
             break;
+        case CoprocStat_mpuCalibrationDone_tag:
+            m_mpu.onCalibrationDone(msg.payload.mpuCalibrationDone);
+            break;
         case CoprocStat_faultStat_tag:
             fault(msg.payload.faultStat);
             break;
@@ -194,13 +197,21 @@ void Manager::sendToCoproc(const CoprocReq& msg) {
     xTaskNotify(m_keepaliveTask, 0, eNoAction);
 }
 
-void Manager::coprocFwVersionAssert(uint32_t minVersion, const char* name) {
+bool Manager::coprocFwAtLeastVersion(uint32_t minVersion) {
     // Ignore zero version number and pass. Might be the cmd was not received yet,
     // but better than to crash in that case.
     if (m_coprocFwVersion.number == 0)
-        return;
+        return true;
+
+    // Ignore dirty versions for development
+    if (m_coprocFwVersion.dirty)
+        return true;
 
-    if (minVersion > m_coprocFwVersion.number) {
+    return minVersion <= m_coprocFwVersion.number;
+}
+
+void Manager::coprocFwVersionAssert(uint32_t minVersion, const char* name) {
+    if (!coprocFwAtLeastVersion(minVersion)) {
         printf("\n\nERROR: Please update your STM32 FW, '%s' requires version "
                "0x%06x and you have 0x%06x!\n\n",
             name, minVersion, m_coprocFwVersion.number);
diff --git a/src/RBCXManager.h b/src/RBCXManager.h
index c3300ec..6e087cd 100644
--- a/src/RBCXManager.h
+++ b/src/RBCXManager.h
@@ -147,6 +147,7 @@ class Manager {
     }
 
     void coprocFwVersionAssert(uint32_t minVersion, const char* name);
+    bool coprocFwAtLeastVersion(uint32_t minVersion);
 
 private:
     Manager();
diff --git a/src/RBCXMpu.cpp b/src/RBCXMpu.cpp
index 07f3c41..91d542f 100644
--- a/src/RBCXMpu.cpp
+++ b/src/RBCXMpu.cpp
@@ -2,15 +2,17 @@
 
 #include "RBCXManager.h"
 
+#define TAG "RBCXMpu"
+
 // Constant to convert raw temperature to Celsius degrees
 static constexpr float MPU6050_TEMP_LINEAR_COEF = 1.0 / 340.00;
 static constexpr float MPU6050_TEMP_OFFSET = 36.53;
 
 // Constant to convert raw gyroscope to degree/s
-static constexpr float MPU_GYRO_FACTOR_250 = 1.0 / 131.0;
+static constexpr float MPU6050_GYRO_FACTOR_250 = 1.0 / 131.0;
 // static constexpr float MPU6050_GYRO_FACTOR_500  = 1.0/65.5;
 // static constexpr float MPU6050_GYRO_FACTOR_1000 = 1.0/32.8;
-// static constexpr float MPU6050_GYRO_FACTOR_2000 = 1.0/16.4;
+static constexpr float MPU6050_GYRO_FACTOR_2000 = 1.0 / 16.4;
 
 // Constant to convert raw acceleration to m/s^2
 // static constexpr float GRAVITATIONAL_CONSTANT_G = 9.81;
@@ -21,13 +23,10 @@ static constexpr float MPU6050_ACCEL_FACTOR_2 = 1.0 / 16384.0;
 // static constexpr float MPU6050_ACCEL_FACTOR_16 = 1 / 2048.0;
 
 static constexpr float RAD_2_DEG = 57.29578; // [°/rad]
-static constexpr int CALIB_OFFSET_NB_MES = 1;
 
 namespace rb {
 
-Mpu::Mpu() {
-    m_lastTicks = xTaskGetTickCount();
-}
+Mpu::Mpu() { m_lastTicks = xTaskGetTickCount(); }
 
 Mpu::~Mpu() {}
 
@@ -59,22 +58,75 @@ void Mpu::setState(const CoprocStat_MpuStat& msg) {
     m_compressCoef = msg.compressCoef;
     calculateAcc(msg.accel);
     calculateGyro(msg.gyro);
-    calculateAngle();
+
+    if (msg.has_yawPitchRoll) {
+        m_mpuMotion.angle.x = msg.yawPitchRoll.x / 16384.f;
+        m_mpuMotion.angle.y = msg.yawPitchRoll.y / 16384.f;
+        m_mpuMotion.angle.z = msg.yawPitchRoll.z / 16384.f;
+    } else {
+        calculateAngle();
+    }
+}
+
+void Mpu::onCalibrationDone(const MpuCalibrationData& data) {
+    if (m_onCalibrationDoneCallback) {
+        std::vector<uint8_t> vec_data(data.data, data.data + sizeof(data.data));
+        m_onCalibrationDoneCallback(vec_data);
+
+        CalibrationDoneCb empty;
+        m_onCalibrationDoneCallback.swap(empty);
+    }
 }
 
 void Mpu::clearCalibrationData() {
-    memset(&m_mpuMotionOffset, 0, sizeof(m_mpuMotionOffset));
+    if (Manager::get().coprocFwAtLeastVersion(0x010300)) {
+        sendMpuReq(CoprocReq_MpuReq {
+            .which_mpuCmd = CoprocReq_MpuReq_restoreCalibrationData_tag,
+            .mpuCmd = {
+                .restoreCalibrationData = {
+                    .data = {},
+                },
+            },
+        });
+    } else {
+        memset(&m_mpuMotionOffset, 0, sizeof(m_mpuMotionOffset));
+    }
 }
 
-void Mpu::setCalibrationData() {
-    m_mpuMotionOffset.accel.x = m_mpuMotion.accel.x / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.accel.y = m_mpuMotion.accel.y / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.accel.z
-        = (m_mpuMotion.accel.z - 1.0) / CALIB_OFFSET_NB_MES;
+void Mpu::restoreCalibrationData(const uint8_t* data, size_t length) {
+    Manager::get().coprocFwVersionAssert(0x010300, "restoreCalibrationData");
+
+    auto req = CoprocReq_MpuReq {
+        .which_mpuCmd = CoprocReq_MpuReq_restoreCalibrationData_tag,
+    };
 
-    m_mpuMotionOffset.gyro.x = m_mpuMotion.gyro.x / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.gyro.y = m_mpuMotion.gyro.y / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.gyro.z = m_mpuMotion.gyro.z / CALIB_OFFSET_NB_MES;
+    if (length != sizeof(req.mpuCmd.restoreCalibrationData.data)) {
+        ESP_LOGE(TAG,
+            "Invalid calibration data length, %d != %d, perhaps from older FW "
+            "version?",
+            length, sizeof(req.mpuCmd.restoreCalibrationData.data));
+        return;
+    }
+
+    memcpy(req.mpuCmd.restoreCalibrationData.data, data, length);
+    sendMpuReq(req);
+}
+
+void Mpu::calibrateNow(CalibrationDoneCb callback) {
+    if (Manager::get().coprocFwAtLeastVersion(0x010300)) {
+        m_onCalibrationDoneCallback = callback;
+        sendMpuReq(CoprocReq_MpuReq {
+            .which_mpuCmd = CoprocReq_MpuReq_calibrateOffsets_tag,
+        });
+    } else {
+        m_mpuMotionOffset.accel.x = m_mpuMotion.accel.x;
+        m_mpuMotionOffset.accel.y = m_mpuMotion.accel.y;
+        m_mpuMotionOffset.accel.z = (m_mpuMotion.accel.z - 1.0);
+
+        m_mpuMotionOffset.gyro.x = m_mpuMotion.gyro.x;
+        m_mpuMotionOffset.gyro.y = m_mpuMotion.gyro.y;
+        m_mpuMotionOffset.gyro.z = m_mpuMotion.gyro.z;
+    }
 }
 
 void Mpu::calculateAcc(const CoprocStat_MpuVector& accel) {
@@ -90,14 +142,15 @@ void Mpu::calculateAcc(const CoprocStat_MpuVector& accel) {
 }
 
 void Mpu::calculateGyro(const CoprocStat_MpuVector& gyro) {
-    m_mpuMotion.gyro.x
-        = (((float)gyro.x / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    const float factor = Manager::get().coprocFwAtLeastVersion(0x010300)
+        ? MPU6050_GYRO_FACTOR_2000
+        : MPU6050_GYRO_FACTOR_250;
+
+    m_mpuMotion.gyro.x = (((float)gyro.x / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.x;
-    m_mpuMotion.gyro.y
-        = (((float)gyro.y / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    m_mpuMotion.gyro.y = (((float)gyro.y / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.y;
-    m_mpuMotion.gyro.z
-        = (((float)gyro.z / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    m_mpuMotion.gyro.z = (((float)gyro.z / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.z;
 }
 
@@ -118,36 +171,32 @@ void Mpu::calculateAngle() {
               sqrt(m_mpuMotion.accel.z * m_mpuMotion.accel.z
                   + m_mpuMotion.accel.y * m_mpuMotion.accel.y))
         * RAD_2_DEG; // [- 90°,+ 90°]
-    
+
     const TickType_t curTicks = xTaskGetTickCount();
-    const float elapsedSeconds = float((curTicks  - m_lastTicks) * portTICK_RATE_MS) / 1000.f;
+    const float elapsedSeconds
+        = float((curTicks - m_lastTicks) * portTICK_RATE_MS) / 1000.f;
     m_lastTicks = curTicks;
 
     // Correctly wrap X and Y angles (special thanks to Edgar Bonet!)
     // https://github.com/gabriel-milan/TinyMPU6050/issues/6
-    m_mpuMotion.angle.x
-        = wrap((m_mpuMotion.angleAcc.x
-                   + wrap( m_mpuMotion.gyro.x * elapsedSeconds
-                           - m_mpuMotion.angleAcc.x,
-                       180))
-                + m_mpuMotion.angleAcc.x,
-            180);
-    m_mpuMotion.angle.y = wrap(
-        (m_mpuMotion.angleAcc.y
-            + wrap(sgZ * m_mpuMotion.gyro.y * elapsedSeconds
-                    - m_mpuMotion.angleAcc.y,
-                90))
-            + m_mpuMotion.angleAcc.y,
-        90);
+    m_mpuMotion.angle.x = wrap(
+        (m_mpuMotion.angleAcc.x
+            + wrap(m_mpuMotion.gyro.x * elapsedSeconds - m_mpuMotion.angleAcc.x,
+                180))
+            + m_mpuMotion.angleAcc.x,
+        180);
+    m_mpuMotion.angle.y
+        = wrap((m_mpuMotion.angleAcc.y
+                   + wrap(sgZ * m_mpuMotion.gyro.y * elapsedSeconds
+                           - m_mpuMotion.angleAcc.y,
+                       90))
+                + m_mpuMotion.angleAcc.y,
+            90);
 
     m_mpuMotion.angle.z
         += m_mpuMotion.gyro.z * elapsedSeconds; // not wrapped (to do???)
 }
 
-void Mpu::resetAngleZ() {
-    m_mpuMotion.angle.z = 0;
-}
-
 /* Wrap an angle in the range [-limit,+limit] (special thanks to Edgar Bonet!) */
 float Mpu::wrap(float angle, float limit) {
     while (angle > limit)
@@ -158,19 +207,8 @@ float Mpu::wrap(float angle, float limit) {
 }
 
 MpuVector Mpu::getAcc() { return m_mpuMotion.accel; }
-float Mpu::getAccX() { return m_mpuMotion.accel.x; }
-float Mpu::getAccY() { return m_mpuMotion.accel.y; }
-float Mpu::getAccZ() { return m_mpuMotion.accel.z; }
-
 MpuVector Mpu::getGyro() { return m_mpuMotion.gyro; }
-float Mpu::getGyroX() { return m_mpuMotion.gyro.x; }
-float Mpu::getGyroY() { return m_mpuMotion.gyro.y; }
-float Mpu::getGyroZ() { return m_mpuMotion.gyro.z; }
-
 MpuVector Mpu::getAngle() { return m_mpuMotion.angle; }
-float Mpu::getAngleX() { return m_mpuMotion.angle.x; }
-float Mpu::getAngleY() { return m_mpuMotion.angle.y; }
-float Mpu::getAngleZ() { return m_mpuMotion.angle.z; }
 
 void Mpu::setCompressCoef(uint8_t coef) {
     sendMpuReq(
diff --git a/src/RBCXMpu.h b/src/RBCXMpu.h
index 40d3ee3..4d93f62 100644
--- a/src/RBCXMpu.h
+++ b/src/RBCXMpu.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <freertos/FreeRTOS.h>
+#include <functional>
+#include <vector>
 
 #include "rbcx.pb.h"
+
 namespace rb {
 
 struct MpuVector {
@@ -34,6 +37,8 @@ class Mpu {
     friend class Manager;
 
 public:
+    typedef std::function<void(const std::vector<uint8_t>&)> CalibrationDoneCb;
+
     /**
      * @brief Initialize the Mpu. This function must be called before using the Mpu.
      * It will send a request to the coprocessor to initialize the Mpu.
@@ -61,39 +66,35 @@ class Mpu {
     void sendStop();
 
     /**
-     * @brief Records current Gyro and Accelerometer data as "start" position
-     * and subtracts it from future readings
+     * @brief Starts MPU calibration procedure
+     * 
+     * If you provide a callback, you can save the data to some persistent storage 
+     * and then use restoreCalibrationData to load them without running the calibration.
      */
-    void setCalibrationData();
+    void calibrateNow(CalibrationDoneCb callback = CalibrationDoneCb());
 
     /**
-     * @brief Clears the data recorded by setCalibrationData
+     * @brief Clears the data recorded by calibrateNow
      */
     void clearCalibrationData();
 
     /**
-     * @brief Get the Mpu acceleration data.
-     * @return The Mpu acceleration data in Gs.
-     */
-    MpuVector getAcc();
-
-    /**
-     * @brief Get the Mpu acceleration data on the X axis.
-     * @return The Mpu acceleration data on the X axis in Gs.
+     * @brief Restores calibration data previously obtained from calibrateNow callback
      */
-    float getAccX();
+    void restoreCalibrationData(const uint8_t* data, size_t length);
 
     /**
-     * @brief Get the Mpu acceleration data on the Y axis.
-     * @return The Mpu acceleration data on the Y axis in Gs.
+     * @brief Restores calibration data previously obtained from calibrateNow callback
      */
-    float getAccY();
+    void restoreCalibrationData(const std::vector<uint8_t>& data) {
+        restoreCalibrationData(data.data(), data.size());
+    }
 
     /**
-     * @brief Get the Mpu acceleration data on the Z axis.
-     * @return The Mpu acceleration data on the Z axis in Gs.
+     * @brief Get the Mpu acceleration data.
+     * @return The Mpu acceleration data in Gs.
      */
-    float getAccZ();
+    MpuVector getAcc();
 
     /**
      * @brief Get the Mpu gyroscope data.
@@ -101,55 +102,12 @@ class Mpu {
      */
     MpuVector getGyro();
 
-    /**
-     * @brief Get the Mpu gyroscope data on the X axis.
-     * @return The Mpu gyroscope data on the X axis in degrees per second.
-     */
-    float getGyroX();
-
-    /**
-     * @brief Get the Mpu gyroscope data on the Y axis.
-     * @return The Mpu gyroscope data on the Y axis in degrees per second.
-     */
-    float getGyroY();
-
-    /**
-     * @brief Get the Mpu gyroscope data on the Z axis.
-     * @return The Mpu gyroscope data on the Z axis in degrees per second.
-     */
-    float getGyroZ();
-
     /**
      * @brief Get the Mpu angle data.
      * @return The Mpu angle data in degrees.
      */
     MpuVector getAngle();
 
-    /**
-     * @brief Get the Mpu angle data on the X axis.
-     * @return The Mpu angle data on the X axis in degrees.
-     */
-    float getAngleX();
-
-    /**
-     * @brief Get the Mpu angle data on the Y axis.
-     * @return The Mpu angle data on the Y axis in degrees.
-     */
-    float getAngleY();
-
-    /**
-     * @brief Get the Mpu angle data on the Z axis.
-     * 
-     * The Z angle is relative to the original heading of the robot at the start of the program,
-     * or after latest resetAngleZ() call, NOT to geographic north.
-     * 
-     * @return The Mpu angle data on the Z axis in degrees.
-     */
-    float getAngleZ();
-
-    // Reset heading back to 0
-    void resetAngleZ();
-
     /**
      * @brief Set the Mpu compression coefficient (10 - 20).
      * It is used to reduce sending interval. The Mpu data are summed up and sent every `interval * coef` ms.
@@ -164,18 +122,13 @@ class Mpu {
     uint8_t getCompressCoef();
 
 private:
-    MpuMotion9 m_mpuMotion;
-    MpuMotion6 m_mpuMotionOffset;
-
-    TickType_t m_lastTicks;
-
-    uint8_t m_compressCoef;
-
     Mpu();
     Mpu(const Mpu&) = delete;
     ~Mpu();
 
     void setState(const CoprocStat_MpuStat& msg);
+    void onCalibrationDone(const MpuCalibrationData& data);
+
     void calculateAcc(const CoprocStat_MpuVector& accel);
     void calculateGyro(const CoprocStat_MpuVector& gyro);
     void calculateAngle();
@@ -183,5 +136,11 @@ class Mpu {
     float wrap(float angle, float limit);
 
     void sendMpuReq(CoprocReq_MpuReq mpuReq);
+
+    CalibrationDoneCb m_onCalibrationDoneCallback;
+    MpuMotion9 m_mpuMotion;
+    MpuMotion6 m_mpuMotionOffset;
+    TickType_t m_lastTicks;
+    uint8_t m_compressCoef;
 };
 };
diff --git a/src/RBCXVersion.h b/src/RBCXVersion.h
index 6b64a4b..bc08c94 100644
--- a/src/RBCXVersion.h
+++ b/src/RBCXVersion.h
@@ -5,5 +5,5 @@
 #define RBCX
 
 #define RB3204_MAJOR 1
-#define RB3204_MINOR 5
+#define RB3204_MINOR 6
 #define RB3204_PATCH 0