More compact module sensors was added

Sensors/Barometer.hpp

@@ -0,0 +1,62 @@
+#include "GyverBME280.h"
+
+class Barometer {
+  public:
+    Barometer(GyverBME280 *bme) {
+        _bme = bme;
+    };
+
+    ~Barometer() {
+        delete this->_bme;
+    }
+
+    bool initialize(void) {
+        _bme->setMode(NORMAL_MODE);
+        _bme->setFilter(FILTER_COEF_16);
+        _bme->setTempOversampling(OVERSAMPLING_2);
+        _bme->setPressOversampling(OVERSAMPLING_16);
+        _bme->setStandbyTime(STANDBY_500US);
+        return _bme->begin();
+    }
+
+    void measureBaseAltitudeSync(void) {
+        for (int i = 0; i < _calibrationIterationsCount; ++i) {
+            _startedAltitude += altitude();
+            delay(_calibrationIterationDelay);
+        }
+        _startedAltitude /= _calibrationIterationsCount;
+    }
+
+    void measureBaseAltitudeAsync(void) {
+        _isStartCalibration = true;
+    }
+
+    float altitude() /* [cm] */ {
+        return pressureToAltitude(_bme->readPressure()) * 100;
+    }
+
+    float flightHeight() /* [cm] */ {
+        return altitude() - _startedAltitude;
+    }
+
+    void tick() {
+        if (_isStartCalibration) {
+            if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
+                _startedAltitude += altitude();
+            }
+            if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
+                _startedAltitude /= _calibrationIterationsCount;
+                _isStartCalibration = false;
+            }
+        }
+    }
+
+  private:
+    GyverBME280 *_bme;
+    float _startedAltitude = 0;
+    bool _isStartCalibration = false;
+    int _calibrationTimer;
+    int _calibrationIterationsCounter = 0;
+    static constexpr int _calibrationIterationsCount = 1500;
+    static constexpr int _calibrationIterationDelay = 1;  // [ms]
+};

Sensors/BatteryController.hpp

@@ -0,0 +1,19 @@
+#include "board_pins.h"
+
+class BatteryController {
+  public:
+    BatteryController() {}
+
+    void initialize() {
+        pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
+        digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
+    }
+
+    float measureVoltage() {
+        return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095;
+    }
+
+    int percent(int minVoltage = 7200, int maxVoltage = 8400) {
+        return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100);
+    }
+};

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;
+    }
+};

Sensors/Sensors.cpp

@@ -0,0 +1,95 @@
+#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;
+
+    ESP_LOGI(_tag, "Initialize BMP280...");
+    if (_barometer->initialize()) {
+        ESP_LOGI(_tag, "BMP280 initialized");
+    } else {
+        ESP_LOGI(_tag, "BMP280 initialization failed!");
+        while (loop_on_fail)
+            ;
+    }
+
+    ESP_LOGI(_tag, "Initialize MPU6050...");
+    if (_mpu->initialize()) {
+        ESP_LOGI(_tag, "MPU6050 initialized");
+    } else {
+        ESP_LOGI(_tag, "MPU6050 initialization failed!");
+        while (loop_on_fail)
+            ;
+    }
+
+    _battery->initialize();
+    ESP_LOGI(_tag, "BatteryController initialized");
+    ESP_LOGI(_tag, "Sensors initialized");
+}
+
+Sensors::~Sensors() {
+    delete _barometer;
+    delete _mpu;
+    delete _2d_filter;
+    delete _battery;
+}
+
+void Sensors::measureBaseAltitudeSync(void) {
+    _barometer->measureBaseAltitudeSync();
+}
+
+void Sensors::measureBaseAltitudeAsync(void) {
+    _barometer->measureBaseAltitudeAsync();
+}
+
+float Sensors::rawFlightHeight(void) {
+    /* Returns flight height from barometer immediatly */
+    return _barometer->flightHeight();
+}
+
+float Sensors::flightHeight(void) {
+    /* Returns flight height from cache immediatly */
+    if (_flightHeightFromBarometer == NAN) {
+        ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
+        return rawFlightHeight();
+    } else {
+        return _flightHeightFromBarometer;
+    }
+}
+
+MpuData Sensors::mpuData() {
+    if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) {
+        ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
+    }
+    return _mpuData;
+}
+
+bool Sensors::tick(void) {
+    /* Super loop iteraion */
+    /* Return true on update */
+    bool err;
+    bool isMpuDataReady = _mpu->tick(err);
+    if (isMpuDataReady and !err) {
+        _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), _flightHeightFromBarometer, _zVelocityAltitude);
+        _mpuData = { .ax = _mpu->ax(),
+                     .ay = _mpu->ay(),
+                     .az = _mpu->az(),
+                     .gx = _mpu->gx(),
+                     .gy = _mpu->gy(),
+                     .gz = _mpu->gz(),
+                     .yaw = _mpu->yaw(),
+                     .pitch = _mpu->pitch(),
+                     .roll = _mpu->roll(),
+                     .gravityZ = _mpu->gravityZ() };
+        return true;
+    } else if (err) {
+        ESP_LOGE(_tag, "Failed to get DMP data!");
+    }
+    return false;
+}
+
+int Sensors::batteryCharge(void) {
+    return _battery->percent();
+}

Sensors/Sensors.hpp

@@ -0,0 +1,46 @@
+#include "Barometer.hpp"
+#include "BatteryController.hpp"
+#include "Filters/Kalman2DFilter.hpp"
+#include "MPU.hpp"
+#include "esp_log.h"
+
+/* Sensors module provides cached and filtered sensors values */
+
+struct MpuData {
+    float ax, ay, az;
+    float gx, gy, gz;
+    float yaw, pitch, roll;
+    float gravityZ;
+};
+
+class Sensors {
+  public:
+    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);
+
+    MpuData mpuData(void);
+
+    int batteryCharge(void);  // [%]
+
+    bool tick(void);
+
+  private:
+    Barometer *_barometer = nullptr;
+    MPU *_mpu = nullptr;
+    BatteryController *_battery = nullptr;
+    Kalman2DFilter *_2d_filter = nullptr;
+
+    /* cached filtered values */
+    float _flightHeightFromBarometer = NAN;
+    float _zVelocityAltitude = NAN;
+    MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
+
+
+    static constexpr const char *_tag = "Sensors";
+};