From 21dbd883f7823d0730c6202dc59bfe7d5d818ad2 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Fri, 5 Jan 2024 20:38:10 +0700 Subject: [PATCH] BluetoothDispatcher was added & reformatting --- Barometer.h | 143 ++++++++++++------------ BatteryController.h | 28 ++--- Filters/Kalman2DFilter.hpp | 51 +++++++++ Filters/kalman.hpp | 46 ++++++++ Filters/median3.hpp | 19 ++++ Filters/runningAverage.hpp | 49 +++++++++ GyverBME280.cpp | 219 ------------------------------------- GyverBME280.h | 112 ------------------- Kalman2DFilter.h | 45 -------- MPU.h | 94 ---------------- MPU.hpp | 100 +++++++++++++++++ RF/BluetoothDispatcher.cpp | 73 +++++++++++++ RF/BluetoothDispatcher.hpp | 42 +++++++ arduino-firmware.ino | 92 ---------------- main.cpp | 56 ++++++++++ 15 files changed, 522 insertions(+), 647 deletions(-) create mode 100644 Filters/Kalman2DFilter.hpp create mode 100644 Filters/kalman.hpp create mode 100644 Filters/median3.hpp create mode 100644 Filters/runningAverage.hpp delete mode 100644 GyverBME280.cpp delete mode 100644 GyverBME280.h delete mode 100644 Kalman2DFilter.h delete mode 100644 MPU.h create mode 100644 MPU.hpp create mode 100644 RF/BluetoothDispatcher.cpp create mode 100644 RF/BluetoothDispatcher.hpp delete mode 100644 arduino-firmware.ino create mode 100644 main.cpp diff --git a/Barometer.h b/Barometer.h index 94b0cd1..02d9cf7 100644 --- a/Barometer.h +++ b/Barometer.h @@ -1,83 +1,84 @@ #include "GyverBME280.h" class Barometer { -public: - Barometer(GyverBME280 *bme) { - _bme = bme; - }; - - ~Barometer() { - delete this->_bme; - } + public: + Barometer(GyverBME280 *bme) { + _bme = bme; + }; - bool initialize(void) { - _bme->setMode(NORMAL_MODE); - _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); + ~Barometer() { + delete this->_bme; } - _startedAltitude /= _calibrationIterationsCount; - } - void measureBaseAltitudeAsync(void) { - _isStartCalibration = true; - } + bool initialize(void) { + _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() { - 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() { - return altitude() - _startedAltitude; - } - - void tick() { - if(_isStartCalibration) { - if(_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) { - _startedAltitude += altitude(); - } - if(++_calibrationIterationsCounter >= _calibrationIterationsCount) { + void measureBaseAltitudeSync(void) { + for (int i = 0; i < _calibrationIterationsCount; ++i) { + _startedAltitude += altitude(); + delay(1); + } _startedAltitude /= _calibrationIterationsCount; - _isStartCalibration = false; - } } - } -private: - GyverBME280 *_bme = nullptr; - float _startedAltitude = 0; - bool _isStartCalibration = false; - int _calibrationTimer; - int _calibrationIterationsCounter = 0; - const int _calibrationIterationsCount = 1500; - const int _calibrationIterationDelay = 1; // [ms] + void measureBaseAltitudeAsync(void) { + _isStartCalibration = true; + } - 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); - } + float rawAltitude() { + 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() { + 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); + } }; \ No newline at end of file diff --git a/BatteryController.h b/BatteryController.h index 68e4411..a116530 100644 --- a/BatteryController.h +++ b/BatteryController.h @@ -1,19 +1,19 @@ #include "board_pins.h" class BatteryController { -public: - BatteryController(){} + public: + BatteryController() {} - void initialize() { - pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT); - digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH); - } + void initialize() { + pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT); + digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH); + } - float measureVoltage() { - return analogRead(BATTERY_DATA_PIN)*3.3/4095; - } - - int percent(int minVoltage=7200, int maxVoltage=8400) { - return map(int(measureVoltage()*1000), minVoltage, maxVoltage, 0, 100); - } - }; \ No newline at end of file + float measureVoltage() { + return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095; + } + + int percent(int minVoltage = 7200, int maxVoltage = 8400) { + return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100); + } +}; \ No newline at end of file diff --git a/Filters/Kalman2DFilter.hpp b/Filters/Kalman2DFilter.hpp new file mode 100644 index 0000000..bb5f44d --- /dev/null +++ b/Filters/Kalman2DFilter.hpp @@ -0,0 +1,51 @@ +#include + +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; +}; \ No newline at end of file diff --git a/Filters/kalman.hpp b/Filters/kalman.hpp new file mode 100644 index 0000000..373fe88 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Filters/median3.hpp b/Filters/median3.hpp new file mode 100644 index 0000000..5c59c6b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Filters/runningAverage.hpp b/Filters/runningAverage.hpp new file mode 100644 index 0000000..fd338a6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/GyverBME280.cpp b/GyverBME280.cpp deleted file mode 100644 index a5b38b5..0000000 --- a/GyverBME280.cpp +++ /dev/null @@ -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(); -} \ No newline at end of file diff --git a/GyverBME280.h b/GyverBME280.h deleted file mode 100644 index a88251e..0000000 --- a/GyverBME280.h +++ /dev/null @@ -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 -#include - -#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 \ No newline at end of file diff --git a/Kalman2DFilter.h b/Kalman2DFilter.h deleted file mode 100644 index 8630515..0000000 --- a/Kalman2DFilter.h +++ /dev/null @@ -1,45 +0,0 @@ -#include - -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; -}; \ No newline at end of file diff --git a/MPU.h b/MPU.h deleted file mode 100644 index 5b92039..0000000 --- a/MPU.h +++ /dev/null @@ -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; - } -}; diff --git a/MPU.hpp b/MPU.hpp new file mode 100644 index 0000000..902bf83 --- /dev/null +++ b/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; + } +}; diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp new file mode 100644 index 0000000..1e1c41c --- /dev/null +++ b/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); + } +} \ No newline at end of file diff --git a/RF/BluetoothDispatcher.hpp b/RF/BluetoothDispatcher.hpp new file mode 100644 index 0000000..017eef6 --- /dev/null +++ b/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 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; +}; \ No newline at end of file diff --git a/arduino-firmware.ino b/arduino-firmware.ino deleted file mode 100644 index 2e2aa46..0000000 --- a/arduino-firmware.ino +++ /dev/null @@ -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); - }*/ -} \ No newline at end of file diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..0fa59af --- /dev/null +++ b/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 + +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); +} \ No newline at end of file