101 lines
2.6 KiB
C++
101 lines
2.6 KiB
C++
#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;
|
|
}
|
|
};
|