From b86af180ab9c28671ab68d0d3118bcc9c0f1de63 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Fri, 5 Jan 2024 23:21:31 +0700 Subject: [PATCH] More compact module sensors was added --- BoardI2C.h | 15 +++ RF/BluetoothDispatcher.cpp | 5 +- RF/BluetoothDispatcher.hpp | 7 +- Barometer.h => Sensors/Barometer.hpp | 32 +------ .../BatteryController.hpp | 0 MPU.hpp => Sensors/MPU.hpp | 40 +++++++- Sensors/Sensors.cpp | 95 +++++++++++++++++++ Sensors/Sensors.hpp | 46 +++++++++ main.cpp | 20 ++-- 9 files changed, 213 insertions(+), 47 deletions(-) create mode 100644 BoardI2C.h rename Barometer.h => Sensors/Barometer.hpp (65%) rename BatteryController.h => Sensors/BatteryController.hpp (100%) rename MPU.hpp => Sensors/MPU.hpp (80%) create mode 100644 Sensors/Sensors.cpp create mode 100644 Sensors/Sensors.hpp diff --git a/BoardI2C.h b/BoardI2C.h new file mode 100644 index 0000000..cd278e3 --- /dev/null +++ b/BoardI2C.h @@ -0,0 +1,15 @@ +#include "Wire.h" +#include "board_pins.h" +#include "esp_log.h" + +class BoardI2C : public TwoWire { + BoardI2C(bool loop_on_fail = true) : TwoWire(0) { + if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)) { + ESP_LOGI("I2CBus", "Bus initialized"); + } else { + ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!"); + while (loop_on_fail) + ; + } + } +}; \ No newline at end of file diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp index 1e1c41c..0291a74 100644 --- a/RF/BluetoothDispatcher.cpp +++ b/RF/BluetoothDispatcher.cpp @@ -30,9 +30,12 @@ void BluetoothDispatcher::tick() { _buffer += (char)_controller->read(); } if (_buffer.endsWith("\r")) { - _serial->println(_buffer.substring(0, _buffer.lastIndexOf("\r"))); + char buffer[_buffer_size]; + _buffer.substring(0, _buffer.lastIndexOf('\r'), buffer); + _serial->println(buffer); // print the payload _buffer.clear(); _controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!")); + // TODO: add callback, that receive new state } if (_buffer.length() > _buffer_size) { _buffer.clear(); diff --git a/RF/BluetoothDispatcher.hpp b/RF/BluetoothDispatcher.hpp index 017eef6..3043db3 100644 --- a/RF/BluetoothDispatcher.hpp +++ b/RF/BluetoothDispatcher.hpp @@ -1,7 +1,6 @@ -#include "Arduino.h" #include "BluetoothSerial.h" #include "HardwareSerial.h" -#include "Stream.h" +#include "mString.h" /* Check the ESP configuration */ #if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED) @@ -37,6 +36,6 @@ class BluetoothDispatcher { bool _confirmRequestDone = false; BluetoothSerial *_controller = nullptr; HardwareSerial *_serial = nullptr; - static constexpr int _buffer_size = 256; - String _buffer; + static constexpr uint16_t _buffer_size = 256; + mString<_buffer_size> _buffer; }; \ No newline at end of file diff --git a/Barometer.h b/Sensors/Barometer.hpp similarity index 65% rename from Barometer.h rename to Sensors/Barometer.hpp index 02d9cf7..f0f4337 100644 --- a/Barometer.h +++ b/Sensors/Barometer.hpp @@ -22,7 +22,7 @@ class Barometer { void measureBaseAltitudeSync(void) { for (int i = 0; i < _calibrationIterationsCount; ++i) { _startedAltitude += altitude(); - delay(1); + delay(_calibrationIterationDelay); } _startedAltitude /= _calibrationIterationsCount; } @@ -31,19 +31,11 @@ class Barometer { _isStartCalibration = true; } - float rawAltitude() { + float altitude() /* [cm] */ { return pressureToAltitude(_bme->readPressure()) * 100; } - float rawFlightHeight() { - return rawAltitude() - _startedAltitude; - } - - float altitude() { - return _filterRA(pressureToAltitude(_bme->readPressure()) * 100); // convert from m to cm - } - - float flightHeight() { + float flightHeight() /* [cm] */ { return altitude() - _startedAltitude; } @@ -65,20 +57,6 @@ class Barometer { bool _isStartCalibration = false; int _calibrationTimer; int _calibrationIterationsCounter = 0; - const int _calibrationIterationsCount = 1500; - const int _calibrationIterationDelay = 1; // [ms] - - float _filterRA(float newVal) { - // running average filter - constexpr auto WINDOW_SIZE = 100; - static int t = 0; - static float vals[WINDOW_SIZE]; - static float average = 0; - if (++t >= WINDOW_SIZE) - t = 0; - average -= vals[t]; - average += newVal; - vals[t] = newVal; - return ((float)average / WINDOW_SIZE); - } + static constexpr int _calibrationIterationsCount = 1500; + static constexpr int _calibrationIterationDelay = 1; // [ms] }; \ No newline at end of file diff --git a/BatteryController.h b/Sensors/BatteryController.hpp similarity index 100% rename from BatteryController.h rename to Sensors/BatteryController.hpp diff --git a/MPU.hpp b/Sensors/MPU.hpp similarity index 80% rename from MPU.hpp rename to Sensors/MPU.hpp index 902bf83..f6fb214 100644 --- a/MPU.hpp +++ b/Sensors/MPU.hpp @@ -2,14 +2,15 @@ #include "board_pins.h" #include "esp_log.h" -volatile bool _isDMPDataReady = false; -void IRAM_ATTR _dmpInterruption() { +static volatile bool _isDMPDataReady = false; +static void IRAM_ATTR _dmpInterruption() { _isDMPDataReady = true; } class MPU { public: MPU(MPU6050_6Axis_MotionApps20 *mpu) { + assert(mpu != nullptr); _mpu = mpu; } @@ -64,6 +65,10 @@ class MPU { _ay = accel.y / 8192.f; _az = accel.z / 8192.f; + _gx = q.x / 32768.f * 250.f; + _gy = q.y / 32768.f * 250.f; + _gz = q.z / 32768.f * 250.f; + _calculateZInertial(gravity); return true; } @@ -82,18 +87,47 @@ class MPU { return degrees(_ypr[2]); }; + float ax() { + return _ax; + } + float ay() { + return _ay; + } + float az() { + return _az; + } + + float gravityZ() { + return _gravity; + } + + + float gx() { + return _gx; + } + float gy() { + return _gy; + } + float gz() { + return _gz; + } + private: MPU6050_6Axis_MotionApps20 *_mpu = nullptr; float _ypr[3]; float _ax, _ay, _az; + float _gx, _gy, _gz; + float _gravity; float _AccZInertial = 0; uint8_t _fifoBuffer[45]; - //volatile bool _isDMPDataReady = false; + const char *_TAG = "MPU6050 module"; void _calculateZInertial(VectorFloat &gravity) { const float anglePitch = _ypr[1]; const float angleRoll = _ypr[2]; + _gravity = gravity.z; + _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + cos(anglePitch) * cos(angleRoll) * _az; _AccZInertial = (_AccZInertial - 1) * gravity.z * 100; } diff --git a/Sensors/Sensors.cpp b/Sensors/Sensors.cpp new file mode 100644 index 0000000..d1b7765 --- /dev/null +++ b/Sensors/Sensors.cpp @@ -0,0 +1,95 @@ +#include "Sensors.hpp" + +Sensors::Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail) { + _barometer = &barometer; + _mpu = &mpu; + _2d_filter = &filter; + _battery = &battery; + + ESP_LOGI(_tag, "Initialize BMP280..."); + if (_barometer->initialize()) { + ESP_LOGI(_tag, "BMP280 initialized"); + } else { + ESP_LOGI(_tag, "BMP280 initialization failed!"); + while (loop_on_fail) + ; + } + + ESP_LOGI(_tag, "Initialize MPU6050..."); + if (_mpu->initialize()) { + ESP_LOGI(_tag, "MPU6050 initialized"); + } else { + ESP_LOGI(_tag, "MPU6050 initialization failed!"); + while (loop_on_fail) + ; + } + + _battery->initialize(); + ESP_LOGI(_tag, "BatteryController initialized"); + ESP_LOGI(_tag, "Sensors initialized"); +} + +Sensors::~Sensors() { + delete _barometer; + delete _mpu; + delete _2d_filter; + delete _battery; +} + +void Sensors::measureBaseAltitudeSync(void) { + _barometer->measureBaseAltitudeSync(); +} + +void Sensors::measureBaseAltitudeAsync(void) { + _barometer->measureBaseAltitudeAsync(); +} + +float Sensors::rawFlightHeight(void) { + /* Returns flight height from barometer immediatly */ + return _barometer->flightHeight(); +} + +float Sensors::flightHeight(void) { + /* Returns flight height from cache immediatly */ + if (_flightHeightFromBarometer == NAN) { + ESP_LOGW(_tag, "flightHeight called, but tick() has called never"); + return rawFlightHeight(); + } else { + return _flightHeightFromBarometer; + } +} + +MpuData Sensors::mpuData() { + if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) { + ESP_LOGW(_tag, "MPU data looks like .tick() has never called"); + } + return _mpuData; +} + +bool Sensors::tick(void) { + /* Super loop iteraion */ + /* Return true on update */ + bool err; + bool isMpuDataReady = _mpu->tick(err); + if (isMpuDataReady and !err) { + _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), _flightHeightFromBarometer, _zVelocityAltitude); + _mpuData = { .ax = _mpu->ax(), + .ay = _mpu->ay(), + .az = _mpu->az(), + .gx = _mpu->gx(), + .gy = _mpu->gy(), + .gz = _mpu->gz(), + .yaw = _mpu->yaw(), + .pitch = _mpu->pitch(), + .roll = _mpu->roll(), + .gravityZ = _mpu->gravityZ() }; + return true; + } else if (err) { + ESP_LOGE(_tag, "Failed to get DMP data!"); + } + return false; +} + +int Sensors::batteryCharge(void) { + return _battery->percent(); +} \ No newline at end of file diff --git a/Sensors/Sensors.hpp b/Sensors/Sensors.hpp new file mode 100644 index 0000000..f41b8fe --- /dev/null +++ b/Sensors/Sensors.hpp @@ -0,0 +1,46 @@ +#include "Barometer.hpp" +#include "BatteryController.hpp" +#include "Filters/Kalman2DFilter.hpp" +#include "MPU.hpp" +#include "esp_log.h" + +/* Sensors module provides cached and filtered sensors values */ + +struct MpuData { + float ax, ay, az; + float gx, gy, gz; + float yaw, pitch, roll; + float gravityZ; +}; + +class Sensors { + public: + Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail = true); + + ~Sensors(); + + void measureBaseAltitudeSync(void); + void measureBaseAltitudeAsync(void); + float rawFlightHeight(void); + float flightHeight(void); + + MpuData mpuData(void); + + int batteryCharge(void); // [%] + + bool tick(void); + + private: + Barometer *_barometer = nullptr; + MPU *_mpu = nullptr; + BatteryController *_battery = nullptr; + Kalman2DFilter *_2d_filter = nullptr; + + /* cached filtered values */ + float _flightHeightFromBarometer = NAN; + float _zVelocityAltitude = NAN; + MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; + + + static constexpr const char *_tag = "Sensors"; +}; \ No newline at end of file diff --git a/main.cpp b/main.cpp index 0fa59af..afbd4f3 100644 --- a/main.cpp +++ b/main.cpp @@ -1,11 +1,11 @@ -#include "Barometer.h" -#include "BatteryController.h" #include "Filters/Kalman2DFilter.hpp" #include "GyverBME280.h" #include "I2Cdev.h" -#include "MPU.hpp" #include "MPU6050_6Axis_MotionApps20.h" #include "RF/BluetoothDispatcher.hpp" +#include "Sensors/Barometer.hpp" +#include "Sensors/BatteryController.hpp" +#include "Sensors/MPU.hpp" #include "Wire.h" #include "board_pins.h" #include @@ -21,20 +21,16 @@ void setup() { Serial.begin(115200); Serial.print("Ininitialize I2C..."); - Serial.println( - i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)); + Serial.println(i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)); Serial.print("Ininitialize BMP280..."); - Serial.println( - barometer.initialize()); + Serial.println(barometer.initialize()); Serial.print("Ininitialize MPU6050..."); - Serial.println( - mpu.initialize()); + Serial.println(mpu.initialize()); Serial.print("Ininitialize Bluetooth..."); - Serial.println( - bluetoothDispatcher.initialize()); + Serial.println(bluetoothDispatcher.initialize()); battery.initialize(); barometer.measureBaseAltitudeSync(); @@ -47,7 +43,7 @@ void loop() { barometer.tick(); if (mpu.tick(err) && !err) { float kalmanAltitude, ZVelocityAltitude; - float barometerAltitude = barometer.rawFlightHeight(); + float barometerAltitude = barometer.flightHeight(); kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude); //Serial.print(barometerAltitude); }