diff --git a/platformio.ini b/platformio.ini index e079902..57db5ba 100644 --- a/platformio.ini +++ b/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 diff --git a/src/App.hpp b/src/App.hpp index 3e3c948..d0eb5ce 100644 --- a/src/App.hpp +++ b/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"; }; \ No newline at end of file diff --git a/src/BoardI2C.hpp b/src/BoardI2C.hpp index a2a7b68..f8442e4 100644 --- a/src/BoardI2C.hpp +++ b/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); } } }; \ No newline at end of file diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp index 7ddc5e2..6da654d 100644 --- a/src/Logic/FlightController.cpp +++ b/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) { diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp index 1d97111..e38c468 100644 --- a/src/Logic/FlightDispatcher.cpp +++ b/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()); } diff --git a/src/Logic/FlightDispatcher.hpp b/src/Logic/FlightDispatcher.hpp index 18a0751..f9bb86b 100644 --- a/src/Logic/FlightDispatcher.hpp +++ b/src/Logic/FlightDispatcher.hpp @@ -7,32 +7,38 @@ #include // 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; diff --git a/src/RF/BluetoothDispatcher.cpp b/src/RF/BluetoothDispatcher.cpp index 6aaab97..f955572 100644 --- a/src/RF/BluetoothDispatcher.cpp +++ b/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); + } } diff --git a/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp index c922113..b1463ba 100644 --- a/src/RF/BluetoothDispatcher.hpp +++ b/src/RF/BluetoothDispatcher.hpp @@ -26,18 +26,26 @@ typedef std::function DeviceConnectedCb; typedef std::function 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; diff --git a/src/Sensors/Barometer.hpp b/src/Sensors/Barometer.hpp index 714df5e..2daf62b 100644 --- a/src/Sensors/Barometer.hpp +++ b/src/Sensors/Barometer.hpp @@ -6,7 +6,7 @@ typedef std::function 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] }; \ No newline at end of file diff --git a/src/Sensors/BatteryController.hpp b/src/Sensors/BatteryController.hpp index d428178..7a78abc 100644 --- a/src/Sensors/BatteryController.hpp +++ b/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"; diff --git a/src/Sensors/RangingSensor.cpp b/src/Sensors/RangingSensor.cpp new file mode 100644 index 0000000..38dd87a --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/src/Sensors/RangingSensor.hpp b/src/Sensors/RangingSensor.hpp new file mode 100644 index 0000000..2bb5874 --- /dev/null +++ b/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 diff --git a/src/Sensors/Sensors.cpp b/src/Sensors/Sensors.cpp index b1eb7ae..34cc7de 100644 --- a/src/Sensors/Sensors.cpp +++ b/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!"); diff --git a/src/Sensors/Sensors.hpp b/src/Sensors/Sensors.hpp index bc54481..874961e 100644 --- a/src/Sensors/Sensors.hpp +++ b/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"; diff --git a/src/board_pins.h b/src/board_pins.h index 925c278..7e8a08e 100644 --- a/src/board_pins.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index d4cce67..4754469 100644 --- a/src/main.cpp +++ b/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))); }