More compact module sensors was added

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