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

All modules and core of the app was created

Browse Source
This commit is contained in:
Goga Coder
2024-01-11 22:47:53 +07:00
parent 3c2889b8f7
commit 5dd404d44b
18 changed files with 647 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
Sensors/Barometer.hpp
View File

@@ -1,5 +1,7 @@
#include "GyverBME280.h"
typedef std::function<void()> OnMeasuringFinishedCb;
class Barometer {
public:
Barometer(GyverBME280 *bme) {
@@ -28,7 +30,7 @@ class Barometer {
}
void measureBaseAltitudeAsync(void) {
_isStartCalibration = true;
_isCalibration = true;
}
float altitude() /* [cm] */ {
@@ -39,14 +41,21 @@ class Barometer {
return altitude() - _startedAltitude;
}
void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) {
_measuaringHeightFinishedCb = callback;
}
void tick() {
if (_isStartCalibration) {
if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
if (_isCalibration) {
if (millis() - _calibrationTimer >= _calibrationIterationDelay) {
_startedAltitude += altitude();
}
if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
_startedAltitude /= _calibrationIterationsCount;
_isStartCalibration = false;
_isCalibration = false;
if (_measuaringHeightFinishedCb) {
_measuaringHeightFinishedCb();
}
}
}
}
@@ -54,9 +63,10 @@ class Barometer {
private:
GyverBME280 *_bme;
float _startedAltitude = 0;
bool _isStartCalibration = false;
int _calibrationTimer;
bool _isCalibration = false;
uint32_t _calibrationTimer;
int _calibrationIterationsCounter = 0;
OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
static constexpr int _calibrationIterationsCount = 1500;
static constexpr int _calibrationIterationDelay = 1; // [ms]
};

174
Sensors/MPU.hpp
View File

@@ -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]);
}
};

41
Sensors/Sensors.cpp
View File

@@ -1,10 +1,15 @@
#include "Sensors.hpp"
Sensors::Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail) {
_barometer = &barometer;
_mpu = &mpu;
_2d_filter = &filter;
_battery = &battery;
Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail) {
assert(barometer != nullptr);
assert(mpu != nullptr);
assert(filter != nullptr);
assert(battery != nullptr);
_barometer = barometer;
_mpu = mpu;
_2d_filter = filter;
_battery = battery;
ESP_LOGI(_tag, "Initialize BMP280...");
if (_barometer->initialize()) {
@@ -44,12 +49,12 @@ void Sensors::measureBaseAltitudeAsync(void) {
_barometer->measureBaseAltitudeAsync();
}
float Sensors::rawFlightHeight(void) {
float Sensors::rawFlightHeight(void) const {
/* Returns flight height from barometer immediatly */
return _barometer->flightHeight();
}
float Sensors::flightHeight(void) {
float Sensors::flightHeight(void) const {
/* Returns flight height from cache immediatly */
if (_flightHeightFromBarometer == NAN) {
ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
@@ -59,8 +64,8 @@ float Sensors::flightHeight(void) {
}
}
MpuData Sensors::mpuData() {
if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) {
MpuData Sensors::mpuData() const {
if (_mpuData.ax == NAN and _mpuData.gravity == NAN) {
ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
}
return _mpuData;
@@ -69,6 +74,7 @@ MpuData Sensors::mpuData() {
bool Sensors::tick(void) {
/* Super loop iteraion */
/* Return true on update */
_barometer->tick();
bool err;
bool isMpuDataReady = _mpu->tick(err);
if (isMpuDataReady and !err) {
@@ -82,7 +88,8 @@ bool Sensors::tick(void) {
.yaw = _mpu->yaw(),
.pitch = _mpu->pitch(),
.roll = _mpu->roll(),
.gravityZ = _mpu->gravityZ() };
.gravity = _mpu->gravityZ(),
.zInertial = _mpu->accZInertial() };
return true;
} else if (err) {
ESP_LOGE(_tag, "Failed to get DMP data!");
@@ -90,6 +97,18 @@ bool Sensors::tick(void) {
return false;
}
int Sensors::batteryCharge(void) {
int Sensors::batteryCharge(void) const {
return _battery->percent();
}
void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) {
_mpu->onCalibrationFinished(callback);
}
void Sensors::startMpuCalibration() {
_mpu->startCalibration();
}
void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
_barometer->onMeasuaringHeightFinished(callback);
}

19
Sensors/Sensors.hpp
View File

@@ -10,23 +10,28 @@ struct MpuData {
float ax, ay, az;
float gx, gy, gz;
float yaw, pitch, roll;
float gravityZ;
float gravity;
float zInertial;
};
class Sensors {
public:
Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail = true);
Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail = true);
~Sensors();
void measureBaseAltitudeSync(void);
void measureBaseAltitudeAsync(void);
float rawFlightHeight(void);
float flightHeight(void);
void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback);
float rawFlightHeight(void) const;
float flightHeight(void) const;
MpuData mpuData(void);
void startMpuCalibration();
void onMpuCalibrationFinished(OnCalibrationFinishedCb callback);
int batteryCharge(void); // [%]
MpuData mpuData(void) const;
int batteryCharge(void) const; // [%]
bool tick(void);
@@ -39,7 +44,7 @@ class Sensors {
/* cached filtered values */
float _flightHeightFromBarometer = NAN;
float _zVelocityAltitude = NAN;
MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
static constexpr const char *_tag = "Sensors";