RangingSensor and bug fixed

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

platformio.ini

@@ -27,7 +27,7 @@ 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}

src/App.hpp

@@ -9,6 +9,7 @@ class Application {
     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,6 +21,7 @@ class Application {
         ESP_LOGI(_tag, "Application destroyed");
         delete _dispatcher;
     }
+
 private:
     FlightDispatcher *_dispatcher = nullptr;
     static constexpr const char *_tag = "Application";

src/BoardI2C.hpp

@@ -2,6 +2,7 @@
 // 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"
@@ -10,12 +11,11 @@ class BoardI2C : public TwoWire {
 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;
+    pkg.s = String(MessageType::PidSettings) + ";" + pkg.s;
-    _bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
+    _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;
+    package.s = String(MessageType::UpdatePackage) + ";" + package.s;
-    _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
+    _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:
     FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
                      FlightController *flightController, volatile bool loop_on_fail = true);
+
     ~FlightDispatcher();
+
     void tick();
+
 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) {
+void BluetoothDispatcher::sendPackage(const char *package) {
-    _controller->write((uint8_t *) package, size);
+    if (!_controller->connected())
+        return;
+    auto res = _controller->println(package);
+    if (res == 0) {
+        delay(10);
+    }
 }
 
 

src/RF/BluetoothDispatcher.hpp

@@ -28,16 +28,24 @@ typedef std::function<void(char *package)> NewPackageCallback;
 class BluetoothDispatcher {
 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:
     void _onConfirmRequest(uint16_t pin);
+
     void _onAuthComplete(boolean success) const;
+
     void _onDeviceConnected(BTAddress device);
 
     const char *_device_name = nullptr;

src/Sensors/Barometer.hpp

@@ -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() {
@@ -65,11 +70,11 @@ class Barometer {
 
 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,6 +3,7 @@
 
 #include "board_pins.h"
 
+
 class BatteryController {
 public:
     BatteryController(const uint16_t batteryPin, const uint16_t battery_data_switch_pin) {

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,
+Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu, Kalman2DFilter *filter,
-                 BatteryController *battery, volatile bool loop_on_fail) {
+                 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 */
+    /* 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 */
+    /* Returns flight height from cache immediately */
-    if (_flightHeightFromBarometer == 0.f) {
-        ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
     return rawFlightHeight();
-    } else {
-        return _flightHeightFromBarometer;
-    }
 }
 
 MpuData Sensors::mpuData() const {
@@ -79,11 +90,14 @@ 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(),
+        //_2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(),
-                           _flightHeightFromBarometer, _zVelocityAltitude);
+        //                   _flightHeightFromBarometer, _zVelocityAltitude);
+        _flightHeightFromBarometer = rawFlightHeight();
+        _zVelocityAltitude = _mpu->accZInertial();
         _mpuData = {.ax = _mpu->ax(),
                 .ay = _mpu->ay(),
                 .az = _mpu->az(),

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,
+    Sensors(Barometer *barometer, RangingSensor *rangingSensor, MPU *mpu, Kalman2DFilter *filter,
-            BatteryController *battery, bool loop_on_fail = true);
+            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;
@@ -44,6 +56,8 @@ class Sensors {
     MPU *_mpu = nullptr;
     BatteryController *_battery = nullptr;
     Kalman2DFilter *_2d_filter = nullptr;
+    RangingSensor *_ranging_sensor = nullptr;
+    HeightSensor _heightSensor;
 
     /* cached filtered values */
     float _flightHeightFromBarometer = 0.f;

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