BluetoothDispatcher was added & reformatting

.clang-format

@@ -1,8 +1,9 @@
 # Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration
 ---
+TabWidth: 2
+UseTab: Never
 AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
-AlignArrayOfStructures: None
 AlignConsecutiveAssignments: None
 AlignConsecutiveBitFields: None
 AlignConsecutiveDeclarations: None
@@ -17,7 +18,6 @@ AllowShortBlocksOnASingleLine: Always
 AllowShortCaseLabelsOnASingleLine: true
 AllowShortEnumsOnASingleLine: true
 AllowShortFunctionsOnASingleLine: Empty
-AllowShortIfStatementsOnASingleLine: AllIfsAndElse
 AllowShortLambdasOnASingleLine: Empty
 AllowShortLoopsOnASingleLine: true
 AlwaysBreakAfterDefinitionReturnType: None
@@ -26,7 +26,6 @@ AlwaysBreakBeforeMultilineStrings: false
 AlwaysBreakTemplateDeclarations: No
 AttributeMacros:
   - __capability
-BasedOnStyle: LLVM
 BinPackArguments: true
 BinPackParameters: true
 BitFieldColonSpacing: Both
@@ -69,7 +68,6 @@ Cpp11BracedListStyle: false
 DeriveLineEnding: true
 DerivePointerAlignment: true
 DisableFormat: false
-EmptyLineAfterAccessModifier: Leave
 EmptyLineBeforeAccessModifier: Leave
 ExperimentalAutoDetectBinPacking: false
 FixNamespaceComments: false
@@ -77,8 +75,6 @@ ForEachMacros:
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
-IfMacros:
-  - KJ_IF_MAYBE
 IncludeBlocks: Preserve
 IncludeCategories:
   - Regex: '^"(llvm|llvm-c|clang|clang-c)/'
@@ -95,10 +91,6 @@ IncludeCategories:
     CaseSensitive: false
 IncludeIsMainRegex: ''
 IncludeIsMainSourceRegex: ''
-IndentAccessModifiers: false
-IndentCaseBlocks: true
-IndentCaseLabels: true
-IndentExternBlock: Indent
 IndentGotoLabels: false
 IndentPPDirectives: None
 IndentRequires: true
@@ -108,7 +100,6 @@ InsertTrailingCommas: None
 JavaScriptQuotes: Leave
 JavaScriptWrapImports: true
 KeepEmptyLinesAtTheStartOfBlocks: true
-LambdaBodyIndentation: Signature
 Language: Cpp
 MacroBlockBegin: ''
 MacroBlockEnd: ''
@@ -119,26 +110,17 @@ ObjCBlockIndentWidth: 2
 ObjCBreakBeforeNestedBlockParam: true
 ObjCSpaceAfterProperty: false
 ObjCSpaceBeforeProtocolList: true
-PPIndentWidth: -1
-PackConstructorInitializers: BinPack
 PenaltyBreakAssignment: 1
 PenaltyBreakBeforeFirstCallParameter: 1
 PenaltyBreakComment: 1
 PenaltyBreakFirstLessLess: 1
-PenaltyBreakOpenParenthesis: 1
 PenaltyBreakString: 1
 PenaltyBreakTemplateDeclaration: 1
 PenaltyExcessCharacter: 1
 PenaltyIndentedWhitespace: 1
 PenaltyReturnTypeOnItsOwnLine: 1
 PointerAlignment: Right
-QualifierAlignment: Leave
-ReferenceAlignment: Pointer
 ReflowComments: false
-RemoveBracesLLVM: false
-SeparateDefinitionBlocks: Leave
-ShortNamespaceLines: 0
-SortIncludes: Never
 SortJavaStaticImport: Before
 SortUsingDeclarations: false
 SpaceAfterCStyleCast: false
@@ -151,26 +133,14 @@ SpaceBeforeCpp11BracedList: false
 SpaceBeforeCtorInitializerColon: true
 SpaceBeforeInheritanceColon: true
 SpaceBeforeParens: ControlStatements
-SpaceBeforeParensOptions:
-  AfterControlStatements: true
-  AfterForeachMacros: true
-  AfterFunctionDefinitionName: false
-  AfterFunctionDeclarationName: false
-  AfterIfMacros: true
-  AfterOverloadedOperator: false
-  BeforeNonEmptyParentheses: false
 SpaceBeforeRangeBasedForLoopColon: true
 SpaceBeforeSquareBrackets: false
 SpaceInEmptyBlock: false
 SpaceInEmptyParentheses: false
 SpacesBeforeTrailingComments: 2
-SpacesInAngles: Leave
 SpacesInCStyleCastParentheses: false
 SpacesInConditionalStatement: false
 SpacesInContainerLiterals: false
-SpacesInLineCommentPrefix:
-  Minimum: 0
-  Maximum: -1
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
 Standard: Auto
@@ -188,3 +158,4 @@ WhitespaceSensitiveMacros:
   - BOOST_PP_STRINGIZE
   - NS_SWIFT_NAME
   - CF_SWIFT_NAME
+IndentWidth: 4

Barometer.h

@@ -1,83 +1,84 @@
 #include "GyverBME280.h"
 
 class Barometer {
-public:
+  public:
-  Barometer(GyverBME280 *bme) {
+    Barometer(GyverBME280 *bme) {
-    _bme = bme;
+        _bme = bme;
-  };
+    };
-  
-  ~Barometer() {
-    delete this->_bme;
-  }
 
-  bool initialize(void) {
+    ~Barometer() {
-    _bme->setMode(NORMAL_MODE);
+        delete this->_bme;
-    _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(1);
     }
-    _startedAltitude /= _calibrationIterationsCount;
-  }
 
-  void measureBaseAltitudeAsync(void) {
+    bool initialize(void) {
-    _isStartCalibration = true;
+        _bme->setMode(NORMAL_MODE);
-  }
+        _bme->setFilter(FILTER_COEF_16);
+        _bme->setTempOversampling(OVERSAMPLING_2);
+        _bme->setPressOversampling(OVERSAMPLING_16);
+        _bme->setStandbyTime(STANDBY_500US);
+        return _bme->begin();
+    }
 
-  float rawAltitude() {
+    void measureBaseAltitudeSync(void) {
-    return pressureToAltitude(_bme->readPressure())*100;
+        for (int i = 0; i < _calibrationIterationsCount; ++i) {
-  }
+            _startedAltitude += altitude();
-
+            delay(1);
-  float rawFlightHeight() {
+        }
-    return rawAltitude() - _startedAltitude;
-  }
-
-  float altitude() {
-    return _filterRA(pressureToAltitude(_bme->readPressure())*100); // convert from m to cm
-  }
-
-  float flightHeight() {
-    return altitude() - _startedAltitude;
-  }
-
-  void tick() {
-    if(_isStartCalibration) {
-      if(_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
-        _startedAltitude += altitude();
-      }
-      if(++_calibrationIterationsCounter >= _calibrationIterationsCount) {
         _startedAltitude /= _calibrationIterationsCount;
-        _isStartCalibration = false;
-      }
     }
-  }
 
-private:
+    void measureBaseAltitudeAsync(void) {
-  GyverBME280 *_bme = nullptr;
+        _isStartCalibration = true;
-  float _startedAltitude = 0;
+    }
-  bool _isStartCalibration = false;
-  int _calibrationTimer;
-  int _calibrationIterationsCounter = 0;
-  const int _calibrationIterationsCount = 1500;
-  const int _calibrationIterationDelay = 1; // [ms]
 
-  float _filterRA(float newVal) {
+    float rawAltitude() {
-    // running average filter
+        return pressureToAltitude(_bme->readPressure()) * 100;
-    constexpr auto WINDOW_SIZE = 100;
+    }
-    static int t = 0;
+
-    static float vals[WINDOW_SIZE];
+    float rawFlightHeight() {
-    static float average = 0;
+        return rawAltitude() - _startedAltitude;
-    if (++t >= WINDOW_SIZE) t = 0;  
+    }
-    average -= vals[t];          
+
-    average += newVal;           
+    float altitude() {
-    vals[t] = newVal;            
+        return _filterRA(pressureToAltitude(_bme->readPressure()) * 100);  // convert from m to cm
-    return ((float)average / WINDOW_SIZE);
+    }
-  }
+
+    float flightHeight() {
+        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;
+    const int _calibrationIterationsCount = 1500;
+    const int _calibrationIterationDelay = 1;  // [ms]
+
+    float _filterRA(float newVal) {
+        // running average filter
+        constexpr auto WINDOW_SIZE = 100;
+        static int t = 0;
+        static float vals[WINDOW_SIZE];
+        static float average = 0;
+        if (++t >= WINDOW_SIZE)
+            t = 0;
+        average -= vals[t];
+        average += newVal;
+        vals[t] = newVal;
+        return ((float)average / WINDOW_SIZE);
+    }
 };

BatteryController.h

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

Filters/Kalman2DFilter.hpp

@@ -0,0 +1,51 @@
+#include <BasicLinearAlgebra.h>
+
+class Kalman2DFilter {
+  public:
+    Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
+        dt /= 1000.f;
+        F = { 1, dt,
+              0, 1 };
+        G = { 0.5f * dt * dt,
+              dt };
+        H = { 1, 0 };
+        I = { 1, 0,
+              0, 1 };
+        Q = G * ~G * accelUncertainty * accelUncertainty;
+        R = { barometerUncertainty * barometerUncertainty };
+        P = { 0, 0,
+              0, 0 };
+        S = { 0,
+              0 };
+    }
+
+    void filter(const float &AccZInertial,
+                const float &AltitudeBarometer,
+                float &AltitudeKalman,
+                float &VelocityVerticalKalman) {
+        Acc = { AccZInertial };
+        S = F * S + G * Acc;
+        P = F * P * ~F + Q;
+        L = H * P * ~H + R;
+        K = P * ~H * Inverse(L);
+        M = { AltitudeBarometer };
+        S = S + K * (M - H * S);
+        AltitudeKalman = S(0, 0);
+        VelocityVerticalKalman = S(1, 0);
+        P = (I - K * H) * P;
+    }
+
+  private:
+    BLA::Matrix<2, 2> F;
+    BLA::Matrix<2, 1> G;
+    BLA::Matrix<2, 2> P;
+    BLA::Matrix<2, 2> Q;
+    BLA::Matrix<2, 1> S;
+    BLA::Matrix<1, 2> H;
+    BLA::Matrix<2, 2> I;
+    BLA::Matrix<1, 1> Acc;
+    BLA::Matrix<2, 1> K;
+    BLA::Matrix<1, 1> R;
+    BLA::Matrix<1, 1> L;
+    BLA::Matrix<1, 1> M;
+};

Filters/kalman.hpp

@@ -0,0 +1,46 @@
+// упрощённый Калман для одномерного случая
+
+#ifndef _GKalman_h
+#define _GKalman_h
+
+class GKalman {
+public:
+    // разброс измерения, разброс оценки, скорость изменения значений
+    GKalman(float mea_e, float est_e, float q) {
+        setParameters(mea_e, est_e, q);
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    GKalman(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // разброс измерения, разброс оценки, скорость изменения значений
+    void setParameters(float mea_e, float est_e, float q) {
+        _err_measure = mea_e;
+        _err_estimate = est_e;
+        _q = q;
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    void setParameters(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // возвращает фильтрованное значение
+    float filtered(float value) {		
+        float _kalman_gain, _current_estimate;
+        _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
+        _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
+        _err_estimate =  (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
+        _last_estimate=_current_estimate;
+        return _current_estimate;
+    }
+
+private:
+    float _err_measure = 0.0;
+    float _err_estimate = 0.0;
+    float _q = 0.0;
+    float _last_estimate = 0.0;
+};
+#endif

Filters/median3.hpp

@@ -0,0 +1,19 @@
+// быстрый медианный фильтр 3-го порядка
+
+#ifndef _GMedian3_h
+#define _GMedian3_h
+
+template < typename TYPE >
+class GMedian3 {
+public:
+    TYPE filtered(TYPE value) {	// возвращает фильтрованное значение
+        buf[_counter] = value;
+        if (++_counter > 2) _counter = 0;
+        return (max(buf[0], buf[1]) == max(buf[1], buf[2])) ? max(buf[0], buf[2]) : max(buf[1], min(buf[0], buf[2]));
+    }
+    
+private:
+    TYPE buf[3];
+    uint8_t _counter = 0;
+};
+#endif

Filters/runningAverage.hpp

@@ -0,0 +1,49 @@
+// экспоненциальное бегущее среднее
+
+#ifndef _GFilterRA_h
+#define _GFilterRA_h
+
+class GFilterRA {
+public:
+    GFilterRA(){}
+    
+    GFilterRA(float coef, uint16_t interval) {
+        _coef = coef;
+        _prd = interval;
+    }
+
+    GFilterRA(float coef) {
+        _coef = coef;
+    }
+
+    void setCoef(float coef) {
+        _coef = coef;
+    }
+    
+    void setPeriod(uint16_t interval) {
+        _prd = interval;
+    }
+
+    float filteredTime(float value) {
+        if (millis() - _tmr >= _prd) {
+            _tmr += _prd;
+            filtered(value);
+        }
+        return _fil;
+    }
+
+    float filtered(float value) {
+        return _fil += (value - _fil) * _coef;
+    }
+    
+    //
+    void setStep(uint16_t interval) {
+        _prd = interval;
+    }
+    
+private:
+    float _coef = 0.0, _fil = 0.0;
+    uint32_t _tmr = 0;
+    uint16_t _prd = 0;
+};
+#endif

GyverBME280.cpp

@@ -1,219 +0,0 @@
-#include "GyverBME280.h"
-
-/* ============ Utilities ============ */
-
-float pressureToAltitude(float pressure) {
-    if (!pressure) return 0;									// If the pressure module has been disabled return '0'
-    pressure /= 100.0F;											// Convert [Pa] to [hPa]
-    return 44330.0f * (1.0f - pow(pressure / 1013.25f, 0.1903f));	// Сalculate altitude
-}
-
-float pressureToMmHg(float pressure) {							
-    return (float)(pressure * 0.00750061683f);					// Convert [Pa] to [mm Hg]
-}
-
-/* ============ Setup & begin ============ */
-
-GyverBME280::GyverBME280(TwoWire &wire) {
-  _wire = &wire;
-}
-
-bool GyverBME280::begin() {
-    return begin(0x76);
-}
-
-bool GyverBME280::begin(uint8_t address) {	
-    _i2c_address = address;
-    /* === Start I2C bus & check BME280 === */
-    if (!reset()) return false;                     // BME280 software reset & ack check
-    uint8_t ID = readRegister(0xD0);
-    if (ID != 0x60 && ID != 0x58) return false;	    // Check chip ID (bme/bmp280)
-    readCalibrationData();       					// Read all calibration values
-
-    /* === Load settings to BME280 === */
-    writeRegister(0xF2, _hum_oversampl);                                              			// write hum oversampling value
-    writeRegister(0xF2, readRegister(0xF2));                             			// rewrite hum oversampling register
-    writeRegister(0xF4, ((_temp_oversampl << 5) | (_press_oversampl << 2) | _operating_mode));    // write temp & press oversampling value , normal mode
-    writeRegister(0xF5, ((_standby_time << 5) | (_filter_coef << 2)));                			// write standby time & filter coef
-    return true;
-}
-
-void GyverBME280::setMode(uint8_t mode) {
-    _operating_mode = mode;
-}
-
-
-void GyverBME280::setFilter(uint8_t mode) {
-    _filter_coef = mode;
-}
-
-void GyverBME280::setStandbyTime(uint8_t mode) {
-    _standby_time = mode;
-}
-
-void GyverBME280::setHumOversampling(uint8_t mode) {
-    _hum_oversampl = mode;
-}
-
-void GyverBME280::setTempOversampling(uint8_t mode) {
-    _temp_oversampl = mode;
-}
-
-void GyverBME280::setPressOversampling(uint8_t mode) {
-    _press_oversampl = mode;
-}
-
-/* ============ Reading ============ */
-
-int32_t GyverBME280::readTempInt(void) {
-    int32_t temp_raw = readRegister24(0xFA);			            // Read 24-bit value
-    if (temp_raw == 0x800000) return 0;								// If the temperature module has been disabled return '0'
-
-    temp_raw >>= 4;													// Start temperature reading in integers
-    int32_t value_1 = ((((temp_raw >> 3) - ((int32_t)CalibrationData._T1 << 1))) *
-    ((int32_t)CalibrationData._T2)) >> 11;
-    int32_t value_2 = (((((temp_raw >> 4) - ((int32_t)CalibrationData._T1)) * 
-    ((temp_raw >> 4) - ((int32_t)CalibrationData._T1))) >> 12) * ((int32_t)CalibrationData._T3)) >> 14;
-
-    return ((int32_t)value_1 + value_2);							// Return temperature in integers
-}
-
-
-float GyverBME280::readTemperature(void) {
-    int32_t temp_raw = readTempInt();
-    float T = (temp_raw * 5 + 128) >> 8;
-    return T / 100.0;												// Return temperature in float
-}
-
-
-float GyverBME280::readPressure(void) {
-    uint32_t press_raw = readRegister24(0xF7);			            // Read 24-bit value
-    if (press_raw == 0x800000) return 0;							// If the pressure module has been disabled return '0'
-
-    press_raw >>= 4;												// Start pressure converting
-    int64_t value_1 = ((int64_t)readTempInt()) - 128000;
-    int64_t value_2 = value_1 * value_1 * (int64_t)CalibrationData._P6;
-    value_2 = value_2 + ((value_1 * (int64_t)CalibrationData._P5) << 17);
-    value_2 = value_2 + (((int64_t)CalibrationData._P4) << 35);
-    value_1 = ((value_1 * value_1 * (int64_t)CalibrationData._P3) >> 8) + ((value_1 * (int64_t)CalibrationData._P2) << 12);
-    value_1 = (((((int64_t)1) << 47) + value_1)) * ((int64_t)CalibrationData._P1) >> 33;
-
-    if (!value_1) return 0; 										// Avoid division by zero
-
-    int64_t p = 1048576 - press_raw;
-    p = (((p << 31) - value_2) * 3125) / value_1;
-    value_1 = (((int64_t)CalibrationData._P9) * (p >> 13) * (p >> 13)) >> 25;
-    value_2 = (((int64_t)CalibrationData._P8) * p) >> 19;
-    p = ((p + value_1 + value_2) >> 8) + (((int64_t)CalibrationData._P7) << 4);
-
-    return (float)p / 256;											// Return pressure in float				
-}
-
-
-float GyverBME280::readHumidity(void) {
-    _wire->beginTransmission(_i2c_address);									// Start I2C transmission
-    _wire->write(0xFD);												   		// Request humidity data register	
-    if (_wire->endTransmission() != 0) return 0;										
-    _wire->requestFrom(_i2c_address, 2);										// Request humidity data 
-    int32_t hum_raw = ((uint16_t)_wire->read() << 8) | (uint16_t)_wire->read();	// Read humidity data 
-    if (hum_raw == 0x8000) return 0;										// If the humidity module has been disabled return '0'
-
-    int32_t value  = (readTempInt() - ((int32_t)76800));		            // Start humidity converting
-    value = (((((hum_raw << 14) - (((int32_t)CalibrationData._H4) << 20) -
-    (((int32_t)CalibrationData._H5) * value)) +((int32_t)16384)) >> 15) * 
-    (((((((value * ((int32_t)CalibrationData._H6)) >> 10) *(((value * 
-    ((int32_t)CalibrationData._H3)) >> 11) + ((int32_t)32768))) >> 10) +
-    ((int32_t)2097152)) * ((int32_t)CalibrationData._H2) + 8192) >> 14));
-    value = (value - (((((value >> 15) * (value >> 15)) >> 7) * ((int32_t)CalibrationData._H1)) >> 4));
-    value = (value < 0) ? 0 : value;
-    value = (value > 419430400) ? 419430400 : value;
-    float h = (value >> 12);
-
-    return h / 1024.0;											// Return humidity in float
-}
-
-/* ============ Misc ============ */
-
-bool GyverBME280::isMeasuring(void)	{								
-    return (bool)((readRegister(0xF3) & 0x08) >> 3);            // Read status register & mask bit "measuring"
-}
-
-void GyverBME280::oneMeasurement(void) {
-    writeRegister(0xF4 , ((readRegister(0xF4) & 0xFC) | 0x02)); // Set the operating mode to FORCED_MODE
-}
-
-/* ============ Private ============ */
-
-/* = BME280 software reset = */
-bool GyverBME280::reset(void) {
-    if (!writeRegister(0x0E , 0xB6)) return false; 
-    delay(10);
-    return true;
-}
-
-
-/* = Read and combine three BME280 registers = */
-uint32_t GyverBME280::readRegister24(uint8_t address) {	 
-    _wire->beginTransmission(_i2c_address);
-    _wire->write(address);
-    if (_wire->endTransmission() != 0) return 0x800000;
-    _wire->requestFrom(_i2c_address, 3);
-    return (((uint32_t)_wire->read() << 16) | ((uint32_t)_wire->read() << 8) | (uint32_t)_wire->read());
-}
-
-
-/* = Write one 8-bit BME280 register = */
-bool GyverBME280::writeRegister(uint8_t address , uint8_t data) {
-    _wire->beginTransmission(_i2c_address);
-    _wire->write(address);
-    _wire->write(data);
-    if (_wire->endTransmission() != 0) return false;
-    return true;
-}
-
-
-/* = Read one 8-bit BME280 register = */
-uint8_t GyverBME280::readRegister(uint8_t address) {
-    _wire->beginTransmission(_i2c_address);
-    _wire->write(address);
-    if (_wire->endTransmission() != 0) return 0;
-    _wire->requestFrom(_i2c_address , 1);
-    return _wire->read();
-}
-
-/* = Structure to store all calibration values = */
-void GyverBME280::readCalibrationData(void) {
-    /* first part request*/
-    _wire->beginTransmission(_i2c_address);
-    _wire->write(0x88);
-    if (_wire->endTransmission() != 0) return;
-    _wire->requestFrom(_i2c_address , 25);
-    /* reading */
-    CalibrationData._T1 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._T2 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._T3 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P1 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P2 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P3 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P4 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P5 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P6 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P7 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P8 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._P9 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._H1 = _wire->read();
-
-    /* second part request*/
-    _wire->beginTransmission(_i2c_address);
-    _wire->write(0xE1);
-    _wire->endTransmission();
-    _wire->requestFrom(_i2c_address , 8);
-    /* reading */
-    CalibrationData._H2 = (_wire->read() | (_wire->read() << 8));
-    CalibrationData._H3 = _wire->read();
-    CalibrationData._H4 = (_wire->read() << 4);
-    uint8_t interVal = _wire->read();
-    CalibrationData._H4 |= (interVal & 0xF);
-    CalibrationData._H5 = (((interVal & 0xF0) >> 4) | (_wire->read() << 4));
-    CalibrationData._H6 = _wire->read();
-}

GyverBME280.h

@@ -1,112 +0,0 @@
-/*
-    Лёгкая библиотека для работы с BME280 по I2C для Arduino
-    Документация: 
-    GitHub: https://github.com/GyverLibs/GyverBME280
-    
-    Egor 'Nich1con' Zaharov for AlexGyver, alex@alexgyver.ru
-    https://alexgyver.ru/
-    MIT License
-
-    Версии:
-    v1.3 - исправлена ошибка при отриц. температуре
-    v1.4 - разбил на h и cpp
-    v1.5 - добавлена поддержка BMP280s
-*/
-
-#ifndef GyverBME280_h
-#define GyverBME280_h
-
-#include <Arduino.h>
-#include <Wire.h>
-
-#define NORMAL_MODE	0x03
-#define FORCED_MODE	0x02
-
-#define STANDBY_500US  0x00
-#define STANDBY_10MS   0x06
-#define STANDBY_20MS   0x07
-#define STANDBY_6250US 0x01
-#define STANDBY_125MS  0x02
-#define STANDBY_250MS  0x03
-#define STANDBY_500MS  0x04
-#define STANDBY_1000MS 0x05
-
-#define MODULE_DISABLE		  0x00
-#define OVERSAMPLING_1        0x01
-#define OVERSAMPLING_2        0x02
-#define OVERSAMPLING_4        0x03
-#define OVERSAMPLING_8        0x04
-#define OVERSAMPLING_16       0x05
-
-#define FILTER_DISABLE  0x00
-#define FILTER_COEF_2   0x01
-#define FILTER_COEF_4   0x02
-#define FILTER_COEF_8   0x03
-#define FILTER_COEF_16  0x04
-
-
-// ================================= CLASS ===================================
-
-class GyverBME280 {
-
-public:
-    GyverBME280(TwoWire &wire);								// Create an object of class BME280
-    bool begin();							// Initialize sensor with standart 0x76 address	
-    bool begin(uint8_t address);				// Initialize sensor with not standart 0x76 address	
-    bool isMeasuring(void);						// Returns 'true' while the measurement is in progress					
-    float readPressure(void);					// Read and calculate atmospheric pressure [float , Pa]
-    float readHumidity(void);					// Read and calculate air humidity [float , %]
-    void oneMeasurement(void);					// Make one measurement and go back to sleep [FORCED_MODE only]
-    void setMode(uint8_t mode);
-    float readTemperature(void);				// Read and calculate air temperature [float , *C]
-    void setFilter(uint8_t mode);				// Adjust the filter ratio other than the standard one [before begin()]
-    void setStandbyTime(uint8_t mode);			// Adjust the sleep time between measurements [NORMAL_MODE only][before begin()]
-    void setHumOversampling(uint8_t mode);		// Set oversampling or disable humidity module [before begin()]
-    void setTempOversampling(uint8_t mode);		// Set oversampling or disable temperature module [before begin()]
-    void setPressOversampling(uint8_t mode);	// Set oversampling or disable pressure module [before begin()]
-
-private:
-
-    //============================== DEFAULT SETTINGS ========================================|
-    TwoWire *_wire = nullptr;
-    int _i2c_address = 0x76;					// BME280 address on I2C bus                  |
-    uint8_t _operating_mode = NORMAL_MODE;		// Sensor operation mode                      |
-    uint8_t _standby_time = STANDBY_250MS;		// Time between measurements in NORMAL_MODE	  |
-    uint8_t _filter_coef = FILTER_COEF_16; 		// Filter ratio IIR                           |
-    uint8_t _temp_oversampl = OVERSAMPLING_4;	// Temperature module oversampling parameter  |
-    uint8_t _hum_oversampl = OVERSAMPLING_1;	// Humidity module oversampling parameter     |
-    uint8_t _press_oversampl = OVERSAMPLING_2;	// Pressure module oversampling parameter     |
-    //========================================================================================|
-    
-    bool reset(void);                                   	// BME280 software reset 
-    int32_t readTempInt();                              	// Temperature reading in integers for the function of reading
-    void readCalibrationData(void);                     	// Read all cells containing calibration values
-    uint8_t readRegister(uint8_t address);					// Read one 8-bit BME280 register
-    uint32_t readRegister24(uint8_t address);        		// Read and combine three BME280 registers
-    bool writeRegister(uint8_t address , uint8_t data);		// Write one 8-bit BME280 register
-
-    struct {												// Structure to store all calibration values
-        uint16_t _T1;
-        int16_t _T2;
-        int16_t _T3;
-        uint16_t _P1;
-        int16_t _P2;
-        int16_t _P3;
-        int16_t _P4;
-        int16_t _P5;
-        int16_t _P6;
-        int16_t _P7;
-        int16_t _P8;
-        int16_t _P9;
-        uint8_t _H1;
-        int16_t _H2;
-        uint8_t _H3;
-        int16_t _H4;
-        int16_t _H5;
-        int8_t _H6;
-    } CalibrationData;
-};
-
-float pressureToMmHg(float pressure);		// Convert [Pa] to [mm Hg]  
-float pressureToAltitude(float pressure);	// Convert pressure to altitude
-#endif

Kalman2DFilter.h

@@ -1,45 +0,0 @@
-#include <BasicLinearAlgebra.h>
-
-class Kalman2DFilter {
-public:
-  Kalman2DFilter(float dt=4.f, float accelUncertainty=10.f, float barometerUncertainty=100.f) {
-    dt /= 1000.f;
-    F = {1, dt,
-        0, 1};
-    G = {0.5f*dt*dt,
-        dt};
-    H = {1,0};
-    I = {1, 0, 
-        0, 1};
-    Q = G * ~G * accelUncertainty*accelUncertainty;
-    R = {barometerUncertainty*barometerUncertainty};
-    P = {0, 0,
-        0, 0};
-    S = {0, 
-        0};
-  }
-
-  void filter(const float &AccZInertial,
-              const float &AltitudeBarometer,
-              float &AltitudeKalman,
-              float &VelocityVerticalKalman) {
-    Acc = {AccZInertial};
-    S = F * S + G * Acc;
-    P = F * P * ~F + Q;
-    L = H * P * ~H + R;
-    K = P * ~H * Inverse(L);
-    M = {AltitudeBarometer};
-    S = S + K * (M - H * S);
-    AltitudeKalman = S(0,0);
-    VelocityVerticalKalman = S(1,0);
-    P = (I - K * H) * P;
-  }
-
-private:
-  BLA::Matrix<2,2> F; BLA::Matrix<2,1> G;
-  BLA::Matrix<2,2> P; BLA::Matrix<2,2> Q;
-  BLA::Matrix<2,1> S; BLA::Matrix<1,2> H;
-  BLA::Matrix<2,2> I; BLA::Matrix<1,1> Acc;
-  BLA::Matrix<2,1> K; BLA::Matrix<1,1> R;
-  BLA::Matrix<1,1> L; BLA::Matrix<1,1> M;
-};

MPU.h

@@ -1,94 +0,0 @@
-#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;
-  }
-};

MPU.hpp

@@ -0,0 +1,100 @@
+#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;
+    }
+};

RF/BluetoothDispatcher.cpp

@@ -0,0 +1,73 @@
+#include "BluetoothDispatcher.hpp"
+
+static DeviceConnectedCb deviceConnectedCallback = nullptr;
+static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param);
+
+BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char *device_name, HardwareSerial *serial) {
+    _device_name = device_name;
+    _controller = controller;
+    _serial = serial;
+}
+
+bool BluetoothDispatcher::initialize() {
+    _controller->enableSSP();
+    _controller->onConfirmRequest([this](uint16_t pin) {
+        _confirmRequestCallback(pin);
+    });
+    _controller->onAuthComplete([this](boolean success) {
+        _authCompleteCallback(success);
+    });
+    _controller->register_callback(deviceConnectedStaticCallback);
+    deviceConnectedCallback = [this](esp_bd_addr_t bt_addr) {
+        _deviceConnectedCallback(bt_addr);
+    };
+    _controller->setTimeout(2);
+    return _controller->begin(_device_name);
+}
+
+void BluetoothDispatcher::tick() {
+    while (_controller->available() and _buffer.length() <= _buffer_size) {
+        _buffer += (char)_controller->read();
+    }
+    if (_buffer.endsWith("\r")) {
+        _serial->println(_buffer.substring(0, _buffer.lastIndexOf("\r")));
+        _buffer.clear();
+        _controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!"));
+    }
+    if (_buffer.length() > _buffer_size) {
+        _buffer.clear();
+    }
+}
+
+BluetoothDispatcher::~BluetoothDispatcher() {
+    _controller->end();
+    delete _controller;
+}
+
+void BluetoothDispatcher::_confirmRequestCallback(uint16_t pin) {
+    _confirmRequestDone = true;
+    _serial->print("The PIN is: ");
+    _serial->println(pin);
+    _controller->confirmReply(true);
+}
+
+void BluetoothDispatcher::_authCompleteCallback(boolean success) {
+    if (success) {
+        _confirmRequestDone = true;
+        _serial->println("Pairing success!");
+    } else {
+        _serial->println("Pairing failed, rejected by user!");
+    }
+}
+
+void BluetoothDispatcher::_deviceConnectedCallback(esp_bd_addr_t bt_addr) {
+    _serial->print("New connection opened: ");
+    _serial->println(BTAddress(bt_addr).toString(true));
+}
+
+
+static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) {
+    if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS and deviceConnectedCallback) {
+        deviceConnectedCallback(param->srv_open.rem_bda);
+    }
+}

RF/BluetoothDispatcher.hpp

@@ -0,0 +1,42 @@
+#include "Arduino.h"
+#include "BluetoothSerial.h"
+#include "HardwareSerial.h"
+#include "Stream.h"
+
+/* Check the ESP configuration */
+#if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+
+
+#if not defined(CONFIG_BT_SPP_ENABLED)
+#error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
+#endif
+
+
+#if not defined(CONFIG_BT_SSP_ENABLED)
+#error Simple Secure Pairing for Bluetooth is not available or not enabled.
+#endif
+/* ************************* */
+
+typedef std::function<void(esp_bd_addr_t bt_addr)> DeviceConnectedCb;
+
+class BluetoothDispatcher {
+  public:
+    BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter", HardwareSerial *serial = &Serial);
+    bool initialize();
+    void tick();
+    ~BluetoothDispatcher();
+
+  private:
+    void _confirmRequestCallback(uint16_t pin);
+    void _authCompleteCallback(boolean success);
+    void _deviceConnectedCallback(esp_bd_addr_t bt_addr);
+
+    const char *_device_name = nullptr;
+    bool _confirmRequestDone = false;
+    BluetoothSerial *_controller = nullptr;
+    HardwareSerial *_serial = nullptr;
+    static constexpr int _buffer_size = 256;
+    String _buffer;
+};

arduino-firmware.ino

@@ -1,92 +0,0 @@
-#include "Barometer.h"
-#include "GyverBME280.h"
-#include "Wire.h"
-#include "board_pins.h"
-#include "I2Cdev.h"
-#include "Kalman2DFilter.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "BatteryController.h"
-#include "MPU.h"
-
-TwoWire i2c = TwoWire(0);
-Barometer barometer(new GyverBME280(i2c));
-MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
-Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
-BatteryController battery;
-
-void setup() {
-  Serial.begin(115200);
-  i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000);
-  Serial.println(barometer.initialize());
-  Serial.println(mpu.initialize());
-  barometer.measureBaseAltitudeSync();
-  battery.initialize();
-  Serial.println("System initialized");
-}
-
-void loop() {
-  /*Serial.print("ax:");
-  Serial.print(ax);
-  Serial.print(",");
-  Serial.print("ay:");
-  Serial.print(ay);
-  Serial.print(",");
-  Serial.print("az:");
-  Serial.print(az);
-  Serial.print(",");*/
-  bool err;
-  barometer.tick();
-  if(mpu.tick(err) && !err) {
-    float kalmanAltitude, ZVelocityAltitude;
-    float barometerAltitude = barometer.rawFlightHeight();
-    kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
-    //Serial.print("Yaw:");
-    //Serial.print(mpu.yaw());
-    //Serial.print(",");
-    //Serial.print("Roll:");
-    //Serial.print(mpu.roll());
-    //Serial.print(",");
-    //Serial.print("Pitch:");
-    //Serial.print(mpu.pitch());
-    //Serial.print(",");
-    //Serial.print("Flight height:");
-    Serial.print(barometerAltitude);
-    Serial.print(",");
-    //Serial.print("Kalman altitude:");
-    Serial.print(kalmanAltitude);
-    Serial.print(",");
-    /*Serial.print(G);
-    Serial.print(",");
-    Serial.print(Pc);
-    Serial.print(",");
-    Serial.print(P);
-    Serial.print(",");
-    Serial.print(Xp);
-    Serial.print(",");*/
-    //Serial.print("Kalman2 altitude: ");
-    Serial.println(ZVelocityAltitude);
-    /*Serial.print("Vertical velocity:");
-    Serial.print(ZVelocityAltitude);
-    Serial.print(",");
-    Serial.print("Battery percent: ");
-    Serial.println(battery.percent());*/
-  }
-  
-  //Serial.print(",");
-  /*
-    //Serial.print("AccZInertial:");
-    Serial.print(AccZInertial);
-    Serial.print(",");
-    //Serial.print("Altitude:");
-    Serial.print(AltitudeBarometer);
-    Serial.print(",");
-    //Serial.print("kAltitude:");
-    Serial.print(AltitudeKalman);
-    Serial.print(",");
-    //Serial.print("Battery voltage: ");
-    //Serial.print(battery.measureVoltage());
-    //Serial.print(",");
-    //Serial.print("kZvelocity:");
-    Serial.println(VelocityVerticalKalman);
-  }*/
-}

main.cpp

@@ -0,0 +1,56 @@
+#include "Barometer.h"
+#include "BatteryController.h"
+#include "Filters/Kalman2DFilter.hpp"
+#include "GyverBME280.h"
+#include "I2Cdev.h"
+#include "MPU.hpp"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "RF/BluetoothDispatcher.hpp"
+#include "Wire.h"
+#include "board_pins.h"
+#include <Arduino.h>
+
+TwoWire i2c = TwoWire(0);
+Barometer barometer(new GyverBME280(i2c));
+MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
+Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
+BatteryController battery;
+BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
+
+void setup() {
+    Serial.begin(115200);
+
+    Serial.print("Ininitialize I2C...");
+    Serial.println(
+      i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
+
+    Serial.print("Ininitialize BMP280...");
+    Serial.println(
+      barometer.initialize());
+
+    Serial.print("Ininitialize MPU6050...");
+    Serial.println(
+      mpu.initialize());
+
+    Serial.print("Ininitialize Bluetooth...");
+    Serial.println(
+      bluetoothDispatcher.initialize());
+
+    battery.initialize();
+    barometer.measureBaseAltitudeSync();
+
+    Serial.println("System initialized");
+}
+
+void loop() {
+    bool err;
+    barometer.tick();
+    if (mpu.tick(err) && !err) {
+        float kalmanAltitude, ZVelocityAltitude;
+        float barometerAltitude = barometer.rawFlightHeight();
+        kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
+        //Serial.print(barometerAltitude);
+    }
+    bluetoothDispatcher.tick();
+    delay(1);
+}