Initial commit

MPU.h

@@ -0,0 +1,94 @@
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "board_pins.h"
+#include "esp_log.h"
+
+volatile bool _isDMPDataReady = false;
+void IRAM_ATTR _dmpInterruption() {_isDMPDataReady = true;}
+
+class MPU {
+public:
+  MPU(MPU6050_6Axis_MotionApps20 *mpu) {
+    _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;
+
+    _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]);};
+
+private:
+  MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
+  float _ypr[3];
+  float _ax, _ay, _az;
+  float _AccZInertial = 0;
+  uint8_t _fifoBuffer[45];
+  //volatile bool _isDMPDataReady = false;
+  const char *_TAG = "MPU6050 module";
+
+  void _calculateZInertial(VectorFloat &gravity) {
+    const float anglePitch = _ypr[1];
+    const float angleRoll = _ypr[2];
+    _AccZInertial = -sin(anglePitch)*_ax +
+                    cos(anglePitch)*
+                    sin(angleRoll)*_ay +
+                    cos(anglePitch)*
+                    cos(angleRoll)*_az;
+    _AccZInertial = (_AccZInertial-1)*gravity.z*100;
+  }
+};