Helicopter/firmware
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Error fixes and base flight logic

Browse Source
This commit is contained in:
Goga Coder
2024-04-03 20:38:44 +07:00
parent 47f91eeae5
commit 0cb666c860
13 changed files with 113 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/Sensors/MPU.hpp
View File

@@ -7,6 +7,7 @@
#include "esp_log.h"
static volatile bool _isDMPDataReady = false;
static void IRAM_ATTR _dmpInterruption() {
_isDMPDataReady = true;
}
@@ -14,7 +15,7 @@ static void IRAM_ATTR _dmpInterruption() {
typedef std::function<void()> OnCalibrationFinishedCb;
class MPU {
public:
public:
MPU(MPU6050_6Axis_MotionApps20 *mpu) {
assert(mpu != nullptr);
_mpu = mpu;
@@ -39,7 +40,7 @@ class MPU {
return false;
}
_mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recommended
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
if (_mpu->dmpInitialize()) {
@@ -99,6 +100,11 @@ class MPU {
_prOffset[0] /= _calibrationsIterCounter;
_prOffset[1] /= _calibrationsIterCounter;
_prOffset[0] = DEG_TO_RAD * (180 - RAD_TO_DEG * _prOffset[0]);
_prOffset[1] = DEG_TO_RAD * (180 - RAD_TO_DEG * _prOffset[1]);
ESP_LOGI(_TAG, "Calibration offsets: roll: %f, pitch: %f", RAD_TO_DEG * _prOffset[1],
RAD_TO_DEG * _prOffset[0]);
_isCalibration = false;
_updateOffsets();
_calibrationsIterCounter = 0;
@@ -113,9 +119,11 @@ class MPU {
_gxOffset += _gx;
_gyOffset += _gy;
_gzOffset += _gz;
_prOffset[0] += _ypr[1];
_prOffset[1] += _ypr[2];
_prOffset[0] += pitch() * DEG_TO_RAD;
_prOffset[1] += roll() * DEG_TO_RAD;
_calibrationsIterCounter++;
ESP_LOGI(_TAG, "Angles on calibration: roll: %f, pitch: %f", roll(),
pitch());
}
}
@@ -126,21 +134,24 @@ class MPU {
float accZInertial() const {
return _AccZInertial;
}
float yaw() const {
return degrees(_ypr[0]);
return degrees(_ypr[0]) + 180.f;
}
float pitch() const {
if (_isCalibration) {
return degrees(_ypr[1]);
return 360.f - (degrees(_ypr[1]) + 180);
} else {
return degrees(_ypr[1] - _prOffset[0]);
return 360.f - (degrees(_ypr[1]) + 180) + degrees(_prOffset[0]);
}
}
float roll() const {
if (_isCalibration) {
return degrees(_ypr[2]);
return 360.f - (degrees(_ypr[2]) + 180);
} else {
return degrees(_ypr[2] - _prOffset[1]);
return 360.f - (degrees(_ypr[2] + _prOffset[1]) + 180) + degrees(_prOffset[1]);
}
}
@@ -151,6 +162,7 @@ class MPU {
return _ax - _axOffset;
}
}
float ay() const {
if (_isCalibration) {
return _ay;
@@ -158,6 +170,7 @@ class MPU {
return _ay - _ayOffset;
}
}
float az() const {
if (_isCalibration) {
return _az;
@@ -177,6 +190,7 @@ class MPU {
return _gx - _gxOffset;
}
}
float gy() const {
if (_isCalibration) {
return _gy;
@@ -184,6 +198,7 @@ class MPU {
return _gy - _gyOffset;
}
}
float gz() const {
if (_isCalibration) {
return _gz;
@@ -200,9 +215,9 @@ class MPU {
_calibrationFinishedCb = callback;
}
private:
private:
MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
float _ypr[3] = { 0 };
float _ypr[3] = {0};
float _ax = 0, _ay = 0, _az = 0;
float _gx = 0, _gy = 0, _gz = 0;
float _gravity = 0;
@@ -210,9 +225,9 @@ class MPU {
float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
float _prOffset[2] = { 0 }; // yaw isn't used
float _prOffset[2] = {0}; // yaw isn't used
uint8_t _fifoBuffer[45] = { 0 };
uint8_t _fifoBuffer[45] = {0};
Preferences _preferences;
const char *_TAG = "MPU6050 module";
@@ -239,8 +254,9 @@ class MPU {
_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);
_prOffset[0] = _preferences.getFloat("pitch", 0.f);
_prOffset[1] = _preferences.getFloat("roll", 0.f);
ESP_LOGI(_TAG, "Offsets loaded: pitch: %f, roll %f", _prOffset[0], _prOffset[1]);
}
void _updateOffsets() {
@@ -252,5 +268,6 @@ class MPU {
_preferences.putFloat("gz", _gzOffset);
_preferences.putFloat("pitch", _prOffset[0]);
_preferences.putFloat("roll", _prOffset[1]);
ESP_LOGI(_TAG, "Offsets saved.");
}
};