More compact module sensors was added

Sensors/MPU.hpp

@@ -0,0 +1,134 @@
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "board_pins.h"
+#include "esp_log.h"
+
+static volatile bool _isDMPDataReady = false;
+static void IRAM_ATTR _dmpInterruption() {
+    _isDMPDataReady = true;
+}
+
+class MPU {
+  public:
+    MPU(MPU6050_6Axis_MotionApps20 *mpu) {
+        assert(mpu != nullptr);
+        _mpu = mpu;
+    }
+
+    ~MPU() {
+        delete _mpu;
+    }
+
+    bool initialize() {
+        _mpu->initialize();
+        delay(250);
+        if (!_mpu->testConnection()) {
+            ESP_LOGE(_TAG, "MPU6050 test connection failed!");
+            return false;
+        }
+        _mpu->setDLPFMode(MPU6050_DLPF_BW_10);  // 10 Hz bandwidth
+        //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
+        //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
+
+        if (_mpu->dmpInitialize()) {
+            ESP_LOGE(_TAG, "Failed to initialize DMP!");
+            return false;
+        }
+        _mpu->setDMPEnabled(true);
+        attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING);
+        return true;
+    }
+
+    bool tick(bool &err) {
+        err = false;
+        if (!_isDMPDataReady) {
+            return false;
+        }
+        if (!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) {
+            ESP_LOGE(_TAG, "Failed to get DMP data!");
+            err = true;
+            return false;
+        }
+
+        Quaternion q;
+        VectorFloat gravity;
+        VectorInt16 accel;
+        VectorInt16 accelReal;
+
+        _mpu->dmpGetQuaternion(&q, _fifoBuffer);
+        _mpu->dmpGetGravity(&gravity, &q);
+        _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
+        _mpu->dmpGetAccel(&accel, _fifoBuffer);
+        _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
+        _isDMPDataReady = false;
+
+        _ax = accel.x / 8192.f;
+        _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;
+
+        _calculateZInertial(gravity);
+        return true;
+    }
+
+    /* getters */
+    float accZInertial() {
+        return _AccZInertial;
+    }
+    float yaw() {
+        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 az() {
+        return _az;
+    }
+
+    float gravityZ() {
+        return _gravity;
+    }
+
+
+    float gx() {
+        return _gx;
+    }
+    float gy() {
+        return _gy;
+    }
+    float gz() {
+        return _gz;
+    }
+
+  private:
+    MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
+    float _ypr[3];
+    float _ax, _ay, _az;
+    float _gx, _gy, _gz;
+    float _gravity;
+    float _AccZInertial = 0;
+    uint8_t _fifoBuffer[45];
+
+    const char *_TAG = "MPU6050 module";
+
+    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;
+    }
+};