RangingSensor and bug fixed

Implemented RangingSensor driver and choice between barometer and RangingSensor. Reduced I/O load.

platformio.ini

@@ -12,10 +12,10 @@
 default_envs = esp32dev
 
 [common]
-lib_deps_builtin = 
+lib_deps_builtin =
     Wire
     BluetoothSerial
-lib_deps_external = 
+lib_deps_external =
     https://git.gogacoder.com/gogacoder/GyverBME280.git @ ~1.5.0
     https://git.gogacoder.com/gogacoder/MPU6050.git @ ~1.0.0
     https://git.gogacoder.com/gogacoder/I2Cdev.git @ ~1.0.1
@@ -27,14 +27,14 @@ lib_deps_external =
     https://github.com/pololu/vl53l0x-arduino @ 1.3.1
 
 [env:esp32dev]
-platform = espressif32
+platform = espressif32 @ 6.5.0
 board = esp32dev
 framework = arduino
 lib_deps = ${common.lib_deps_external}
 monitor_speed = 115200
 board_build.partitions = huge_app.csv
-build_src_flags = 
-    -std=c++2a 
+build_src_flags =
+    -std=c++2a
     -Wall
     -Wextra
     -Werror=all

src/App.hpp

@@ -5,10 +5,11 @@
 #include "esp_log.h"
 
 class Application {
-  public:
+public:
     Application(FlightDispatcher *dispatcher) {
         assert(dispatcher != nullptr);
         _dispatcher = dispatcher;
+        ESP_LOGI(_tag, "ESP_IDF version: %s", esp_get_idf_version());
         ESP_LOGI(_tag, "Application startup complete");
     }
 
@@ -20,7 +21,8 @@ class Application {
         ESP_LOGI(_tag, "Application destroyed");
         delete _dispatcher;
     }
-  private:
+
+private:
     FlightDispatcher *_dispatcher = nullptr;
     static constexpr const char *_tag = "Application";
 };

src/BoardI2C.hpp

@@ -2,20 +2,20 @@
 // PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
 
 #define CONFIG_DISABLE_HAL_LOCKS 0
+
 #include "Wire.h"
 #include "board_pins.h"
 #include "esp_log.h"
 
 class BoardI2C : public TwoWire {
-  public:
+public:
     BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
         if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
-            setTimeOut(50);
+            setTimeOut(10);
             ESP_LOGI("I2CBus", "Bus initialized");
         } else {
             ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");
-            while (loop_on_fail)
-                ;
+            while (loop_on_fail);
         }
     }
 };

src/Logic/FlightController.cpp

@@ -49,6 +49,7 @@ void FlightController::tick() {
             break;
         case DeviceStatus::IsBoarding:
             /* TODO: implement boarding */
+            _boarding();
             break;
         case DeviceStatus::IsFlying:
             /* TODO: implement flying */
@@ -169,6 +170,7 @@ void FlightController::stopAllRotors() {
     setRotor1Duty(0);
     setRotor2Duty(0);
     setRotor3Duty(0);
+    _status = DeviceStatus::Idle;
 }
 
 void FlightController::setHeightControllerParams(float p, float i, float d) {

src/Logic/FlightDispatcher.cpp

@@ -167,8 +167,8 @@ void FlightDispatcher::_pidSettingsOpened() {
 
     pkg.endObj();
     pkg.end();
-    pkg.s = String(MessageType::PidSettings) + ";" + pkg.s + _message_delimeter;
-    _bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
+    pkg.s = String(MessageType::PidSettings) + ";" + pkg.s;
+    _bluetoothDispatcher->sendPackage(pkg.s.c_str());
     ESP_LOGI(_tag, "PID settings sended %s", pkg.s.c_str());
 }
 
@@ -186,6 +186,6 @@ void FlightDispatcher::_sendTelemetry() {
     package["zIn"] = state.mpuState.zInertial;
     package.endObj();
     package.end();
-    package.s = String(MessageType::UpdatePackage) + ";" + package.s + _message_delimeter;
-    _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
+    package.s = String(MessageType::UpdatePackage) + ";" + package.s;
+    _bluetoothDispatcher->sendPackage(package.s.c_str());
 }

src/Logic/FlightDispatcher.hpp

@@ -7,32 +7,38 @@
 #include <map>
 
 // Message type annotation for mobile app
-enum MessageType { UpdatePackage = 0, PidSettings };
+enum MessageType {
+    UpdatePackage = 0, PidSettings
+};
 
 class FlightDispatcher {
     /* Deserialize state and update it in FlightController. */
-  public:
+public:
     FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
                      FlightController *flightController, volatile bool loop_on_fail = true);
+
     ~FlightDispatcher();
+
     void tick();
-  private:
+
+private:
     /* Telemetry flow */
     void _sendTelemetry();
 
     /* Events handlers */
     void _onNewDeviceConnected(BTAddress device);
+
     void _onNewMessageReceived(char *package);
 
     /* Requests handlers */
     void _changeStatus(const DeviceStatus &newStatus);
+
     void _pidSettingsOpened();
 
     /* Compile time settings */
     static constexpr const char *_tag = "FlightDispatcher";
     static constexpr const int _telemetryTimeIntervalMS = 200;
     static constexpr const uint8_t _jsonMaxDepth = 5;
-    static constexpr const char *_message_delimeter = "\n";
 
     uint32_t _telemetryTimer = millis();
     BluetoothDispatcher *_bluetoothDispatcher;

src/RF/BluetoothDispatcher.cpp

@@ -27,7 +27,7 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
     deviceConnectedCallback = [this](BTAddress device) {
         _onDeviceConnected(device);
     };
-    _controller->setTimeout(50);
+    _controller->setTimeout(readTimeoutMS);
     bool success = _controller->begin(_device_name, false);
     if (!success) {
         ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
@@ -40,7 +40,7 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
 }
 
 void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageReceivedCb) {
-    _newPackageReceivedCb = newPackageReceivedCb;
+    _newPackageReceivedCb = std::move(newPackageReceivedCb);
 }
 
 void BluetoothDispatcher::tick(const char message_delimeter) {
@@ -93,11 +93,16 @@ void BluetoothDispatcher::_onDeviceConnected(BTAddress device) {
 
 void BluetoothDispatcher::onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb) {
     /* Callback called if device connected successfully. */
-    _deviceConnectedCallback = deviceConnectedCb;
+    _deviceConnectedCallback = std::move(deviceConnectedCb);
 }
 
-void BluetoothDispatcher::sendPackage(const char *package, size_t size) {
-    _controller->write((uint8_t *) package, size);
+void BluetoothDispatcher::sendPackage(const char *package) {
+    if (!_controller->connected())
+        return;
+    auto res = _controller->println(package);
+    if (res == 0) {
+        delay(10);
+    }
 }
 
 

src/RF/BluetoothDispatcher.hpp

@@ -26,18 +26,26 @@ typedef std::function<void(BTAddress device)> DeviceConnectedCb;
 typedef std::function<void(char *package)> NewPackageCallback;
 
 class BluetoothDispatcher {
-  public:
+public:
     BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
-    bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2);
+
+    bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 10);
+
     void tick(const char message_delimeter = '\n');
+
     void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
+
     void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
-    void sendPackage(const char *package, size_t size);
+
+    void sendPackage(const char *package);
+
     ~BluetoothDispatcher();
 
-  private:
+private:
     void _onConfirmRequest(uint16_t pin);
+
     void _onAuthComplete(boolean success) const;
+
     void _onDeviceConnected(BTAddress device);
 
     const char *_device_name = nullptr;

src/Sensors/Barometer.hpp

@@ -6,7 +6,7 @@
 typedef std::function<void()> OnMeasuringFinishedCb;
 
 class Barometer {
-  public:
+public:
     Barometer(GyverBME280 *bme) {
         _bme = bme;
     };
@@ -33,7 +33,10 @@ class Barometer {
     }
 
     void measureBaseAltitudeAsync(void) {
+        _calibrationIterationsCounter = 0;
+        _calibrationTimer = millis();
         _isCalibration = true;
+        _startedAltitude = 0.f;
     }
 
     float altitude() /* [cm] */ {
@@ -41,11 +44,13 @@ class Barometer {
     }
 
     float flightHeight() /* [cm] */ {
-        return altitude() - _startedAltitude;
+        if (_isCalibration)
+            return 0.f;
+        else return round(altitude() - _startedAltitude);
     }
 
     void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) {
-        _measuaringHeightFinishedCb = callback;
+        _measuaringHeightFinishedCb = std::move(callback);
     }
 
     void tick() {
@@ -63,13 +68,13 @@ class Barometer {
         }
     }
 
-  private:
+private:
     GyverBME280 *_bme;
-    float _startedAltitude = 0;
+    float _startedAltitude = 0.f;
     bool _isCalibration = false;
     uint32_t _calibrationTimer = 0;
     int _calibrationIterationsCounter = 0;
     OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
-    static constexpr int _calibrationIterationsCount = 1500;
+    static constexpr int _calibrationIterationsCount = 1000;
     static constexpr int _calibrationIterationDelay = 1;  // [ms]
 };

src/Sensors/BatteryController.hpp

@@ -3,8 +3,9 @@
 
 #include "board_pins.h"
 
+
 class BatteryController {
-  public:
+public:
     BatteryController(const uint16_t batteryPin, const uint16_t battery_data_switch_pin) {
         _battery_pin = batteryPin;
         _battery_data_switch_pin = battery_data_switch_pin;
@@ -29,7 +30,7 @@ class BatteryController {
         return constrain(round(percent), 0, 100);
     }
 
-  private:
+private:
     uint16_t _battery_pin;
     uint16_t _battery_data_switch_pin;
     static constexpr const char *_tag = "BatteryController";

src/Sensors/RangingSensor.cpp

@@ -0,0 +1,43 @@
+//
+// Created by gogacoder on 10.03.24.
+//
+
+#include "RangingSensor.hpp"
+#include "esp_log.h"
+
+RangingSensor::RangingSensor(TwoWire &i2c, volatile bool loop_on_fail) {
+    _sensor = VL53L0X();
+    _sensor.setBus(&i2c);
+    if (_sensor.init()) {
+        _sensor.setMeasurementTimingBudget(20000);
+        _sensor.setTimeout(25);
+        ESP_LOGI(_tag, "Sensor initialized");
+    } else {
+        ESP_LOGE(_tag, "Failed to initialize RangingSensor!");
+        while (loop_on_fail) {}
+    }
+    _sensor.startContinuous(10);
+}
+
+void RangingSensor::startFlight() {
+    _start_distance = _distance;
+}
+
+void RangingSensor::tick() {
+    _distance = (uint32_t) round(runMiddleArifm((float) _sensor.readRangeContinuousMillimeters())) / 10;
+}
+
+uint32_t RangingSensor::getDistance() {
+    return _distance - _start_distance;
+}
+
+float RangingSensor::runMiddleArifm(float newVal) {
+    static int t = 0;
+    static float vals[_MID_ARITM_WINDOW];
+    static float average = 0;
+    if (++t >= _MID_ARITM_WINDOW) t = 0; // перемотка t
+    average -= vals[t];         // вычитаем старое
+    average += newVal;          // прибавляем новое
+    vals[t] = newVal;           // запоминаем в массив
+    return ((float) average / _MID_ARITM_WINDOW);
+}

src/Sensors/RangingSensor.hpp

@@ -0,0 +1,33 @@
+//
+// Created by gogacoder on 10.03.24.
+//
+
+#ifndef HELICOPTER_FIRMWARE_RANGINGSENSOR_H
+#define HELICOPTER_FIRMWARE_RANGINGSENSOR_H
+
+#include "Wire.h"
+#include "VL53L0X.h"
+
+class RangingSensor {
+public:
+    RangingSensor(TwoWire &i2c, volatile bool loop_on_fail = true);
+
+    void tick();
+
+    void startFlight();
+
+    uint32_t getDistance();
+
+private:
+    TwoWire *_i2c;
+    VL53L0X _sensor;
+    uint32_t _distance; // [cm]
+    uint32_t _start_distance = 0; // [cm]
+    static constexpr const char *_tag = "RangingSensor";
+    constexpr static const int _MID_ARITM_WINDOW = 5;
+
+    float runMiddleArifm(float newVal);
+};
+
+
+#endif //HELICOPTER_FIRMWARE_RANGINGSENSOR_H

src/Sensors/Sensors.cpp

@@ -3,25 +3,27 @@
 
 #include "Sensors.hpp"
 
-Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
-                 BatteryController *battery, volatile bool loop_on_fail) {
+Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu, Kalman2DFilter *filter,
+                 BatteryController *battery, HeightSensor heightSensor, volatile bool loop_on_fail) {
     assert(barometer != nullptr);
     assert(mpu != nullptr);
     assert(filter != nullptr);
     assert(battery != nullptr);
+    assert(ranging_sensor != nullptr);
 
     _barometer = barometer;
     _mpu = mpu;
     _2d_filter = filter;
     _battery = battery;
+    _ranging_sensor = ranging_sensor;
+    _heightSensor = heightSensor;
 
     ESP_LOGI(_tag, "Initialize BMP280...");
     if (_barometer->initialize()) {
         ESP_LOGI(_tag, "BMP280 initialized");
     } else {
         ESP_LOGI(_tag, "BMP280 initialization failed!");
-        while (loop_on_fail)
-            ;
+        while (loop_on_fail);
     }
 
     ESP_LOGI(_tag, "Initialize MPU6050...");
@@ -29,12 +31,12 @@ Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
         ESP_LOGI(_tag, "MPU6050 initialized");
     } else {
         ESP_LOGI(_tag, "MPU6050 initialization failed!");
-        while (loop_on_fail)
-            ;
+        while (loop_on_fail);
     }
 
     _battery->initialize();
     ESP_LOGI(_tag, "BatteryController initialized");
+
     ESP_LOGI(_tag, "Sensors initialized");
 }
 
@@ -43,6 +45,7 @@ Sensors::~Sensors() {
     delete _mpu;
     delete _2d_filter;
     delete _battery;
+    delete _ranging_sensor;
 }
 
 void Sensors::measureBaseAltitudeSync(void) {
@@ -54,18 +57,26 @@ void Sensors::measureBaseAltitudeAsync(void) {
 }
 
 float Sensors::rawFlightHeight(void) const {
-    /* Returns flight height from barometer immediatly */
-    return _barometer->flightHeight();
+    /* Returns flight height from barometer immediately */
+    switch (_heightSensor) {
+        case HeightSensor::Hybrid:
+            if (_barometer->flightHeight() >= 200.f) {
+                return _barometer->flightHeight();
+            } else {
+                return (float) _ranging_sensor->getDistance();
+            }
+        case HeightSensor::BarometerOnly:
+            return _barometer->flightHeight();
+        case HeightSensor::RangeMeterOnly:
+            return (float) _ranging_sensor->getDistance();
+        default:
+            return _barometer->flightHeight();
+    }
 }
 
 float Sensors::flightHeight(void) const {
-    /* Returns flight height from cache immediatly */
-    if (_flightHeightFromBarometer == 0.f) {
-        ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
-        return rawFlightHeight();
-    } else {
-        return _flightHeightFromBarometer;
-    }
+    /* Returns flight height from cache immediately */
+    return rawFlightHeight();
 }
 
 MpuData Sensors::mpuData() const {
@@ -79,22 +90,25 @@ bool Sensors::tick(void) {
     /* Super loop iteraion */
     /* Return true on update */
     _barometer->tick();
+    _ranging_sensor->tick();
     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(),
-                     .gravity = _mpu->gravityZ(),
-                     .zInertial = _mpu->accZInertial() };
+        //_2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(),
+        //                   _flightHeightFromBarometer, _zVelocityAltitude);
+        _flightHeightFromBarometer = rawFlightHeight();
+        _zVelocityAltitude = _mpu->accZInertial();
+        _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(),
+                .gravity = _mpu->gravityZ(),
+                .zInertial = _mpu->accZInertial()};
         return true;
     } else if (err) {
         ESP_LOGE(_tag, "Failed to get DMP data!");

src/Sensors/Sensors.hpp

@@ -4,6 +4,7 @@
 #include "Barometer.hpp"
 #include "BatteryController.hpp"
 #include "Filters/Kalman2DFilter.hpp"
+#include "RangingSensor.hpp"
 #include "MPU.hpp"
 #include "esp_log.h"
 
@@ -17,20 +18,31 @@ struct MpuData {
     float zInertial;
 };
 
+enum HeightSensor {
+    RangeMeterOnly,
+    BarometerOnly,
+    Hybrid
+};
+
 class Sensors {
-  public:
-    Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
-            BatteryController *battery, bool loop_on_fail = true);
+public:
+    Sensors(Barometer *barometer, RangingSensor *rangingSensor, MPU *mpu, Kalman2DFilter *filter,
+            BatteryController *battery, HeightSensor heightSensor = Hybrid, bool loop_on_fail = true);
 
     ~Sensors();
 
     void measureBaseAltitudeSync(void);
+
     void measureBaseAltitudeAsync(void);
+
     void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback);
+
     float rawFlightHeight(void) const;
+
     float flightHeight(void) const;
 
     void startMpuCalibration();
+
     void onMpuCalibrationFinished(OnCalibrationFinishedCb callback);
 
     MpuData mpuData(void) const;
@@ -39,16 +51,18 @@ class Sensors {
 
     bool tick(void);
 
-  private:
+private:
     Barometer *_barometer = nullptr;
     MPU *_mpu = nullptr;
     BatteryController *_battery = nullptr;
     Kalman2DFilter *_2d_filter = nullptr;
+    RangingSensor *_ranging_sensor = nullptr;
+    HeightSensor _heightSensor;
 
     /* cached filtered values */
     float _flightHeightFromBarometer = 0.f;
     float _zVelocityAltitude = 0.f;
-    MpuData _mpuData = { 0.f };
+    MpuData _mpuData = {0.f};
 
 
     static constexpr const char *_tag = "Sensors";

src/board_pins.h

@@ -1,8 +1,6 @@
 // This is a personal academic project. Dear PVS-Studio, please check it.
 // PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
 
-#include "driver/adc.h"
-
 #define I2C_SDA_PIN 21
 #define I2C_SCL_PIN 18
 #define MPU6050_INT_PIN 19

src/main.cpp

@@ -14,14 +14,18 @@ void setup() {
             new BluetoothDispatcher(new BluetoothSerial()),
             new FlightController(
                     new Sensors(new Barometer(new GyverBME280(i2c)),
+                                new RangingSensor(i2c),
                                 new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
                                 new Kalman2DFilter(10.f, 1.f, 1.8f),
-                                new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN)),
+                                new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN),
+                                HeightSensor::Hybrid),
                     new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightControl"),  // height
                     new SavedPidRegulator(2.f, 12.f, 0.f, "YawControl"),  // yaw
                     new BrushedMotor(1, 2, 3, 4, 1),
                     new BrushedMotor(1, 2, 3, 4, 1),
-                    new PIDController(0.6f, 3.5f, 0.03f, new BrushedMotor(1, 2, 3, 4, 3), "RollControl"),  // pitch
+                    new PIDController(0.6f, 3.5f, 0.03f,
+                                      new BrushedMotor(1, 2, 3, 4, 3),
+                                      "RollControl"),  // pitch
                     true)));
 }