More compact module sensors was added

BoardI2C.h

@@ -0,0 +1,15 @@
+#include "Wire.h"
+#include "board_pins.h"
+#include "esp_log.h"
+
+class BoardI2C : public TwoWire {
+    BoardI2C(bool loop_on_fail = true) : TwoWire(0) {
+        if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)) {
+            ESP_LOGI("I2CBus", "Bus initialized");
+        } else {
+            ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");
+            while (loop_on_fail)
+                ;
+        }
+    }
+};

RF/BluetoothDispatcher.cpp

@@ -30,9 +30,12 @@ void BluetoothDispatcher::tick() {
         _buffer += (char)_controller->read();
     }
     if (_buffer.endsWith("\r")) {
-        _serial->println(_buffer.substring(0, _buffer.lastIndexOf("\r")));
+        char buffer[_buffer_size];
+        _buffer.substring(0, _buffer.lastIndexOf('\r'), buffer);
+        _serial->println(buffer);  // print the payload
         _buffer.clear();
         _controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!"));
+        // TODO: add callback, that receive new state
     }
     if (_buffer.length() > _buffer_size) {
         _buffer.clear();

RF/BluetoothDispatcher.hpp

@@ -1,7 +1,6 @@
-#include "Arduino.h"
 #include "BluetoothSerial.h"
 #include "HardwareSerial.h"
-#include "Stream.h"
+#include "mString.h"
 
 /* Check the ESP configuration */
 #if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED)
@@ -37,6 +36,6 @@ class BluetoothDispatcher {
     bool _confirmRequestDone = false;
     BluetoothSerial *_controller = nullptr;
     HardwareSerial *_serial = nullptr;
-    static constexpr int _buffer_size = 256;
-    String _buffer;
+    static constexpr uint16_t _buffer_size = 256;
+    mString<_buffer_size> _buffer;
 };

Sensors/Barometer.hpp

@@ -22,7 +22,7 @@ class Barometer {
     void measureBaseAltitudeSync(void) {
         for (int i = 0; i < _calibrationIterationsCount; ++i) {
             _startedAltitude += altitude();
-            delay(1);
+            delay(_calibrationIterationDelay);
         }
         _startedAltitude /= _calibrationIterationsCount;
     }
@@ -31,19 +31,11 @@ class Barometer {
         _isStartCalibration = true;
     }
 
-    float rawAltitude() {
+    float altitude() /* [cm] */ {
         return pressureToAltitude(_bme->readPressure()) * 100;
     }
 
-    float rawFlightHeight() {
-        return rawAltitude() - _startedAltitude;
-    }
-
-    float altitude() {
-        return _filterRA(pressureToAltitude(_bme->readPressure()) * 100);  // convert from m to cm
-    }
-
-    float flightHeight() {
+    float flightHeight() /* [cm] */ {
         return altitude() - _startedAltitude;
     }
 
@@ -65,20 +57,6 @@ class Barometer {
     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);
-    }
+    static constexpr int _calibrationIterationsCount = 1500;
+    static constexpr int _calibrationIterationDelay = 1;  // [ms]
 };

Sensors/MPU.hpp

@@ -2,14 +2,15 @@
 #include "board_pins.h"
 #include "esp_log.h"
 
-volatile bool _isDMPDataReady = false;
-void IRAM_ATTR _dmpInterruption() {
+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;
     }
 
@@ -64,6 +65,10 @@ 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;
+
         _calculateZInertial(gravity);
         return true;
     }
@@ -82,18 +87,47 @@ class MPU {
         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];
-    //volatile bool _isDMPDataReady = false;
+
     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";
+};

main.cpp

@@ -1,11 +1,11 @@
-#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 "Sensors/Barometer.hpp"
+#include "Sensors/BatteryController.hpp"
+#include "Sensors/MPU.hpp"
 #include "Wire.h"
 #include "board_pins.h"
 #include <Arduino.h>
@@ -21,20 +21,16 @@ void setup() {
     Serial.begin(115200);
 
     Serial.print("Ininitialize I2C...");
-    Serial.println(
-      i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
+    Serial.println(i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
 
     Serial.print("Ininitialize BMP280...");
-    Serial.println(
-      barometer.initialize());
+    Serial.println(barometer.initialize());
 
     Serial.print("Ininitialize MPU6050...");
-    Serial.println(
-      mpu.initialize());
+    Serial.println(mpu.initialize());
 
     Serial.print("Ininitialize Bluetooth...");
-    Serial.println(
-      bluetoothDispatcher.initialize());
+    Serial.println(bluetoothDispatcher.initialize());
 
     battery.initialize();
     barometer.measureBaseAltitudeSync();
@@ -47,7 +43,7 @@ void loop() {
     barometer.tick();
     if (mpu.tick(err) && !err) {
         float kalmanAltitude, ZVelocityAltitude;
-        float barometerAltitude = barometer.rawFlightHeight();
+        float barometerAltitude = barometer.flightHeight();
         kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
         //Serial.print(barometerAltitude);
     }