All modules and core of the app was created

Sensors/MPU.hpp

@@ -1,4 +1,5 @@
 #include "MPU6050_6Axis_MotionApps20.h"
+#include "Preferences.h"
 #include "board_pins.h"
 #include "esp_log.h"
 
@@ -7,6 +8,8 @@ static void IRAM_ATTR _dmpInterruption() {
     _isDMPDataReady = true;
 }
 
+typedef std::function<void()> OnCalibrationFinishedCb;
+
 class MPU {
   public:
     MPU(MPU6050_6Axis_MotionApps20 *mpu) {
@@ -20,6 +23,13 @@ class MPU {
 
     bool initialize() {
         _mpu->initialize();
+
+        if (!_preferences.begin("imu")) {
+            ESP_LOGE(_TAG, "Failed to open the preferences!");
+        } else {
+            _loadOffsets();
+        }
+
         delay(250);
         if (!_mpu->testConnection()) {
             ESP_LOGE(_TAG, "MPU6050 test connection failed!");
@@ -52,12 +62,14 @@ class MPU {
         Quaternion q;
         VectorFloat gravity;
         VectorInt16 accel;
+        VectorInt16 gyro;
         VectorInt16 accelReal;
 
         _mpu->dmpGetQuaternion(&q, _fifoBuffer);
         _mpu->dmpGetGravity(&gravity, &q);
         _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
         _mpu->dmpGetAccel(&accel, _fifoBuffer);
+        _mpu->dmpGetGyro(&gyro, _fifoBuffer);
         _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
         _isDMPDataReady = false;
 
@@ -65,51 +77,121 @@ class MPU {
         _ay = accel.y / 8192.f;
         _az = accel.z / 8192.f;
 
-        _gx = q.x / 32768.f * 250.f;
-        _gy = q.y / 32768.f * 250.f;
-        _gz = q.z / 32768.f * 250.f;
+        _gx = gyro.x / 131.f;
+        _gy = gyro.y / 131.f;
+        _gz = gyro.z / 131.f;
 
         _calculateZInertial(gravity);
+
+        if (_isCalibration) {
+            if (_calibrationsIterCounter >= _calibrationIterCount) {
+                _axOffset /= _calibrationsIterCounter;
+                _ayOffset /= _calibrationsIterCounter;
+                _azOffset /= _calibrationsIterCounter;
+
+                _gxOffset /= _calibrationsIterCounter;
+                _gyOffset /= _calibrationsIterCounter;
+                _gzOffset /= _calibrationsIterCounter;
+
+                _prOffset[0] /= _calibrationsIterCounter;
+                _prOffset[1] /= _calibrationsIterCounter;
+
+                _isCalibration = false;
+                _updateOffsets();
+                if (_calibrationFinishedCb) {
+                    _calibrationFinishedCb();
+                }
+            } else {
+                _axOffset += _ax;
+                _ayOffset += _ay;
+                _azOffset += _az;
+                _gxOffset += _gx;
+                _gyOffset += _gy;
+                _gzOffset += _gz;
+                _prOffset[0] += _ypr[1];
+                _prOffset[1] += _ypr[2];
+            }
+        }
+
         return true;
     }
 
     /* getters */
-    float accZInertial() {
+    float accZInertial() const {
         return _AccZInertial;
     }
-    float yaw() {
+    float yaw() const {
         return degrees(_ypr[0]);
-    };
-    float pitch() {
-        return degrees(_ypr[1]);
-    };
-    float roll() {
-        return degrees(_ypr[2]);
-    };
-
-    float ax() {
-        return _ax;
     }
-    float ay() {
-        return _ay;
+    float pitch() const {
+        if (_isCalibration) {
+            return degrees(_ypr[1]);
+        } else {
+            return degrees(_ypr[1] - _yprOffset[1]);
+        }
     }
-    float az() {
-        return _az;
+    float roll() const {
+        if (_isCalibration) {
+            return degrees(_ypr[2]);
+        } else {
+            return degrees(_ypr[2] - _yprOffset[2]);
+        }
     }
 
-    float gravityZ() {
+    float ax() const {
+        if (_isCalibration) {
+            return _ax;
+        } else {
+            return _ax - _axOffset;
+        }
+    }
+    float ay() const {
+        if (_isCalibration) {
+            return _ay;
+        } else {
+            return _ay - _ayOffset;
+        }
+    }
+    float az() const {
+        if (_isCalibration) {
+            return _az;
+        } else {
+            return _az - _azOffset;
+        }
+    }
+
+    float gravityZ() const {
         return _gravity;
     }
 
+    float gx() const {
+        if (_isCalibration) {
+            return _gx;
+        } else {
+            return _gx - _gxOffset;
+        }
+    }
+    float gy() const {
+        if (_isCalibration) {
+            return _gy;
+        } else {
+            return _gy - _gyOffset;
+        }
+    }
+    float gz() const {
+        if (_isCalibration) {
+            return _gz;
+        } else {
+            return _gz - _gzOffset;
+        }
+    }
 
-    float gx() {
-        return _gx;
+    void startCalibration() {
+        _isCalibration = true;
     }
-    float gy() {
-        return _gy;
-    }
-    float gz() {
-        return _gz;
+
+    void onCalibrationFinished(OnCalibrationFinishedCb callback) {
+        _calibrationFinishedCb = callback;
     }
 
   private:
@@ -119,16 +201,50 @@ class MPU {
     float _gx, _gy, _gz;
     float _gravity;
     float _AccZInertial = 0;
+
+    float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
+    float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
+    float _prOffset[2]; // yaw isn't used
+
     uint8_t _fifoBuffer[45];
+    Preferences _preferences;
 
     const char *_TAG = "MPU6050 module";
+    static constexpr const int _calibrationIterCount = 250;
+
+    bool _isCalibration = false;
+    int _calibrationsIterCounter = 0;
+    OnCalibrationFinishedCb _calibrationFinishedCb = nullptr;
 
     void _calculateZInertial(VectorFloat &gravity) {
         const float anglePitch = _ypr[1];
         const float angleRoll = _ypr[2];
         _gravity = gravity.z;
 
-        _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + cos(anglePitch) * cos(angleRoll) * _az;
-        _AccZInertial = (_AccZInertial - 1) * gravity.z * 100;
+        _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay
+                        + cos(anglePitch) * cos(angleRoll) * _az;
+        _AccZInertial = (_AccZInertial - 1) * gravity.z * 100.f;
+    }
+
+    void _loadOffsets() {
+        _axOffset = _preferences.getFloat("ax", 0.f);
+        _ayOffset = _preferences.getFloat("ax", 0.f);
+        _azOffset = _preferences.getFloat("az", 0.f);
+        _gxOffset = _preferences.getFloat("gx", 0.f);
+        _gyOffset = _preferences.getFloat("gy", 0.f);
+        _gzOffset = _preferences.getFloat("gz", 0.f);
+        _ypr[1] = _preferences.getFloat("pitch", 0.f);
+        _ypr[2] = _preferences.getFloat("roll", 0.f);
+    }
+
+    void _updateOffsets() {
+        _preferences.putFloat("ax", _axOffset);
+        _preferences.putFloat("ay", _ayOffset);
+        _preferences.putFloat("az", _azOffset);
+        _preferences.putFloat("gx", _gxOffset);
+        _preferences.putFloat("gy", _gyOffset);
+        _preferences.putFloat("gz", _gzOffset);
+        _preferences.putFloat("pitch", _prOffset[0]);
+        _preferences.putFloat("roll", _prOffset[1]);
     }
 };