From 5dd404d44b4ef25f8fd14023e9fef563c581d3eb Mon Sep 17 00:00:00 2001 From: gogacoder Date: Thu, 11 Jan 2024 22:47:53 +0700 Subject: [PATCH] All modules and core of the app was created --- .clang-format | 3 +- App.hpp | 23 +++++ BoardI2C.h => BoardI2C.hpp | 3 +- Filters/Kalman2DFilter.hpp | 20 ++--- Logic/FlightController.cpp | 104 ++++++++++++++++++++++ Logic/FlightController.hpp | 54 ++++++++++++ Logic/FlightDispatcher.cpp | 80 +++++++++++++++++ Logic/FlightDispatcher.hpp | 24 +++++ Logic/PID.hpp | 50 +++++++++++ Motor/BrushedMotor.cpp | 22 +++++ Motor/BrushedMotor.hpp | 29 +++++++ RF/BluetoothDispatcher.cpp | 23 +++-- RF/BluetoothDispatcher.hpp | 12 +-- Sensors/Barometer.hpp | 22 +++-- Sensors/MPU.hpp | 174 ++++++++++++++++++++++++++++++------- Sensors/Sensors.cpp | 41 ++++++--- Sensors/Sensors.hpp | 19 ++-- main.cpp | 72 ++++++--------- 18 files changed, 647 insertions(+), 128 deletions(-) create mode 100644 App.hpp rename BoardI2C.h => BoardI2C.hpp (84%) create mode 100644 Logic/FlightController.cpp create mode 100644 Logic/FlightController.hpp create mode 100644 Logic/FlightDispatcher.cpp create mode 100644 Logic/FlightDispatcher.hpp create mode 100644 Logic/PID.hpp create mode 100644 Motor/BrushedMotor.cpp create mode 100644 Motor/BrushedMotor.hpp diff --git a/.clang-format b/.clang-format index f4b061b..9f3311b 100644 --- a/.clang-format +++ b/.clang-format @@ -1,7 +1,6 @@ # Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration --- TabWidth: 2 -UseTab: Never AccessModifierOffset: -2 AlignAfterOpenBracket: Align AlignConsecutiveAssignments: None @@ -58,7 +57,7 @@ BreakConstructorInitializers: BeforeColon BreakConstructorInitializersBeforeComma: false BreakInheritanceList: BeforeColon BreakStringLiterals: false -ColumnLimit: 120 +ColumnLimit: 90 CommentPragmas: '' CompactNamespaces: false ConstructorInitializerAllOnOneLineOrOnePerLine: false diff --git a/App.hpp b/App.hpp new file mode 100644 index 0000000..c597f7d --- /dev/null +++ b/App.hpp @@ -0,0 +1,23 @@ +#include "Logic/FlightDispatcher.hpp" +#include "esp_log.h" + +class Application { + public: + Application(FlightDispatcher *dispatcher) { + assert(dispatcher != nullptr); + _dispatcher = dispatcher; + ESP_LOGI(_tag, "Application startup complete"); + } + + void tick() { + _dispatcher->tick(); + } + + ~Application() { + ESP_LOGI(_tag, "Application destroyed"); + delete _dispatcher; + } + private: + FlightDispatcher *_dispatcher = nullptr; + static constexpr const char *_tag = "Application"; +}; \ No newline at end of file diff --git a/BoardI2C.h b/BoardI2C.hpp similarity index 84% rename from BoardI2C.h rename to BoardI2C.hpp index cd278e3..d6a98ba 100644 --- a/BoardI2C.h +++ b/BoardI2C.hpp @@ -3,8 +3,9 @@ #include "esp_log.h" class BoardI2C : public TwoWire { + public: BoardI2C(bool loop_on_fail = true) : TwoWire(0) { - if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)) { + if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) { ESP_LOGI("I2CBus", "Bus initialized"); } else { ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!"); diff --git a/Filters/Kalman2DFilter.hpp b/Filters/Kalman2DFilter.hpp index bb5f44d..e3e8ee2 100644 --- a/Filters/Kalman2DFilter.hpp +++ b/Filters/Kalman2DFilter.hpp @@ -4,25 +4,17 @@ 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 }; + F = { 1, dt, 0, 1 }; + G = { 0.5f * dt * dt, dt }; H = { 1, 0 }; - I = { 1, 0, - 0, 1 }; + I = { 1, 0, 0, 1 }; Q = G * ~G * accelUncertainty * accelUncertainty; R = { barometerUncertainty * barometerUncertainty }; - P = { 0, 0, - 0, 0 }; - S = { 0, - 0 }; + P = { 0, 0, 0, 0 }; + S = { 0, 0 }; } - void filter(const float &AccZInertial, - const float &AltitudeBarometer, - float &AltitudeKalman, - float &VelocityVerticalKalman) { + void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) { Acc = { AccZInertial }; S = F * S + G * Acc; P = F * P * ~F + Q; diff --git a/Logic/FlightController.cpp b/Logic/FlightController.cpp new file mode 100644 index 0000000..40e5276 --- /dev/null +++ b/Logic/FlightController.cpp @@ -0,0 +1,104 @@ +#include "FlightController.hpp" + +FlightController::FlightController(Sensors *sensors, PIDController *pid1, + PIDController *pid2, PIDController *pid3, bool unlimitedBattery) { + assert(sensors != nullptr); + assert(pid1 != nullptr); + assert(pid2 != nullptr); + assert(pid3 != nullptr); + _sensors = sensors; + _unlimitedBattery = unlimitedBattery; + + _status = DeviceStatus::Idle; + _updateBatteryCharge(); + ESP_LOGI(_tag, "Flight controller initialized"); +} + +FlightController::~FlightController() { + delete _sensors; + delete _pid1; + delete _pid2; + delete _pid3; +} + +void FlightController::tick() { + bool hasUpdates = _sensors->tick(); + if (hasUpdates) { + _updateBatteryCharge(); + } + switch (_status) { + case DeviceStatus::Idle: break; + case DeviceStatus::ChargeRequired: break; + case DeviceStatus::IsPreparingForTakeoff: break; + case DeviceStatus::IsBoarding: + /* TODO: implement boarding */ + break; + case DeviceStatus::IsFlying: + /* TODO: implement flying */ + break; + } +} + +ReportedDeviceState FlightController::currentDeviceState() const { + return { .batteryCharge = _sensors->batteryCharge(), + .flightHeight = _sensors->flightHeight(), + .status = _status, + .mpuState = _sensors->mpuData() }; +} + +void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) { + /* Start preparation for takeoff */ + _updateBatteryCharge(); + if (_status == DeviceStatus::ChargeRequired) + return; + + _status = DeviceStatus::IsPreparingForTakeoff; + _sensors->onMeasuaringAltitudeFinished([this]() { + if (_status == DeviceStatus::IsPreparingForTakeoff) { + // Preparation for takeoff is done + _takeoff(); + } + }); + _sensors->measureBaseAltitudeAsync(); +} + +void FlightController::startBoarding() { + if (_status == DeviceStatus::IsFlying) { + _status = DeviceStatus::IsBoarding; + } +} + +void FlightController::_updateBatteryCharge() { + if(_unlimitedBattery) return; + if (_sensors->batteryCharge() <= _batteryCriticalCharge) { + if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) { + // TODO: board the device + _status = DeviceStatus::IsBoarding; + } else { + _status = DeviceStatus::ChargeRequired; + } + } +} + +void FlightController::_takeoff() { + // TODO: implement takeoff + _status = DeviceStatus::IsFlying; + if(_onTakeoffStarted) { + _onTakeoffStarted(); + } +} + +void FlightController::_boarding() { + if (_sensors->flightHeight() <= _defaultStopMotorHeight) { + // TODO: stop motors and setpoints + _status = DeviceStatus::Idle; + } else { + // TODO: implement boarding + } +} + +void FlightController::startImuCalibration() { + _sensors->onMpuCalibrationFinished([this]() { + this->_status = DeviceStatus::Idle; + }); +} \ No newline at end of file diff --git a/Logic/FlightController.hpp b/Logic/FlightController.hpp new file mode 100644 index 0000000..b337325 --- /dev/null +++ b/Logic/FlightController.hpp @@ -0,0 +1,54 @@ +#include "PID.hpp" +#include "Sensors/Sensors.hpp" + +enum DeviceStatus { + Idle = 0, + IsPreparingForTakeoff, + IsFlying, + IsBoarding, + IsImuCalibration, + ChargeRequired +}; + +struct ReportedDeviceState { + int batteryCharge; + float flightHeight; + DeviceStatus status; + MpuData mpuState; +}; + +struct FlightParams { + float height; // [cm] + float yaw; // [degrees] + float pitch; // [degrees] +}; + +class FlightController { + public: + FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, PIDController *pid3, bool unlimitedBattery = false); + ~FlightController(); + ReportedDeviceState currentDeviceState() const; + void startTakeoff(OnMeasuringFinishedCb onTakeoffStarted); + void startBoarding(); + void startImuCalibration(); + void tick(); + private: + void _updateBatteryCharge(); + void _takeoff(); + void _boarding(); + + Sensors *_sensors = nullptr; + PIDController *_pid1 = nullptr; + PIDController *_pid2 = nullptr; + PIDController *_pid3 = nullptr; + DeviceStatus _status; + OnMeasuringFinishedCb _onTakeoffStarted = nullptr; + + static constexpr const char *_tag = "FlightController"; + static constexpr uint8_t _batteryCriticalCharge = 15; // [%] + static constexpr float _defaultFlightHeight = 30.f; // [cm] + static constexpr float _defaultHorizontAngle = 90.f; // [degrees] + static constexpr float _defaultStopMotorHeight = 5; // [cm] + + bool _unlimitedBattery = false; // for debug purposes +}; \ No newline at end of file diff --git a/Logic/FlightDispatcher.cpp b/Logic/FlightDispatcher.cpp new file mode 100644 index 0000000..dcf2a81 --- /dev/null +++ b/Logic/FlightDispatcher.cpp @@ -0,0 +1,80 @@ +#include "FlightDispatcher.hpp" + +FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher, + FlightController *flightController, bool loop_on_fail) { + assert(bluetoothDispatcher != nullptr); + assert(flightController != nullptr); + + _bluetoothDispatcher = bluetoothDispatcher; + _flightController = flightController; + + bluetoothDispatcher->onNewDeviceConnected([this](BTAddress device) { + this->_onNewDeviceConnected(device); + }); + bluetoothDispatcher->onNewPackageReceived([this](char *package) { + this->_onNewMessageReceived(package); + }); + + if (_bluetoothDispatcher->initialize()) { + ESP_LOGI(_tag, "Bluetooth initialized successfully."); + } else { + ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!"); + while (loop_on_fail) + ; + } + _telemetryTimer = millis(); +} + +FlightDispatcher::~FlightDispatcher() { + delete _bluetoothDispatcher; + delete _flightController; +} + +void FlightDispatcher::tick() { + _flightController->tick(); + _bluetoothDispatcher->tick(); + if (millis() - _telemetryTimer >= _telemetryTimeIntervalMS) { + _sendTelemetry(); + _telemetryTimer = millis(); + } +} + +void FlightDispatcher::_onNewDeviceConnected(BTAddress device) { + auto currentState = _flightController->currentDeviceState(); + gson::string package; + package.beginObj(); + package["batteryCharge"] = currentState.batteryCharge; + package["flightHeight"] = currentState.flightHeight; + package["status"] = currentState.status; + package.endObj(); + package.end(); + package += "\r\n"; + auto pkg = package.s.c_str(); + _bluetoothDispatcher->sendPackage(pkg, strlen(pkg)); +} + +void FlightDispatcher::_onNewMessageReceived(char *package) { + ESP_LOGI(_tag, "Received new package: %s", package); + gson::Doc pkg; + pkg.parse(package, _jsonMaxDepth); + // TODO: process package +} + +void FlightDispatcher::_sendTelemetry() { + /* Notify mobile client about device state */ + auto state = _flightController->currentDeviceState(); + gson::string package; + package.beginObj(); + package["batteryCharge"] = state.batteryCharge; + package["flightHeight"] = state.flightHeight; + package["status"] = state.status; + package["y"] = state.mpuState.yaw; + package["p"] = state.mpuState.pitch; + package["r"] = state.mpuState.roll; + package["zIn"] = state.mpuState.zInertial; + package.endObj(); + package.end(); + package += "\r\n"; + + _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str()))); +} diff --git a/Logic/FlightDispatcher.hpp b/Logic/FlightDispatcher.hpp new file mode 100644 index 0000000..c6a833f --- /dev/null +++ b/Logic/FlightDispatcher.hpp @@ -0,0 +1,24 @@ +#include "FlightController.hpp" +#include "GSON.h" +#include "RF/BluetoothDispatcher.hpp" + +class FlightDispatcher { + /* Deserialize state and update it in FlightController. */ + public: + FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher, + FlightController *flightController, bool loop_on_fail = true); + ~FlightDispatcher(); + void tick(); + private: + void _onNewDeviceConnected(BTAddress device); + void _onNewMessageReceived(char *package); + void _sendTelemetry(); + + static constexpr const char *_tag = "FlightDispatcher"; + static constexpr const int _telemetryTimeIntervalMS = 200; + static constexpr const uint8_t _jsonMaxDepth = 5; + + uint32_t _telemetryTimer = millis(); + BluetoothDispatcher *_bluetoothDispatcher; + FlightController *_flightController; +}; \ No newline at end of file diff --git a/Logic/PID.hpp b/Logic/PID.hpp new file mode 100644 index 0000000..28d1578 --- /dev/null +++ b/Logic/PID.hpp @@ -0,0 +1,50 @@ +#include "GyverPID.h" +#include "Motor/BrushedMotor.hpp" +#include "Preferences.h" +#include "esp_log.h" +#include "mString.h" + +class PIDController : public GyverPID { + public: + PIDController(float p, float i, float d, BrushedMotor *rotor, int rotorNumber = 1, uint16_t dt = 10) + : GyverPID(p, i, d, dt) { + assert(rotor != nullptr); + _rotor = rotor; + _rotorNumber = rotorNumber; + setLimits(0, _rotor->maxDuty()); + _preferences.begin((String("r") + String(rotorNumber)).c_str()); + _loadPreferences(p, i, d); + pid_timer = millis(); + ESP_LOGI(_tag, "PID №%d initialized with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd); + } + + void tick() { + if (millis() - pid_timer >= _dt) { + _rotor->setDuty(getResultTimer()); + pid_timer = millis(); + } + } + + void save() { + _preferences.putFloat("p", Kp); + _preferences.putFloat("i", Ki); + _preferences.putFloat("d", Kd); + ESP_LOGI(_tag, "PID №%d saved with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd); + } + + private: + Preferences _preferences; + int _rotorNumber = 1; + uint16_t _dt; + BrushedMotor *_rotor = nullptr; + uint32_t pid_timer = 0; + static constexpr const char *_tag = "PIDController"; + + void _loadPreferences(float pDefault = 0, float iDefault = 0, float dDefault = 0) { + if (_preferences.isKey("p") and _preferences.isKey("i") and _preferences.isKey("d")) { + Kp = _preferences.getFloat("p", pDefault); + Ki = _preferences.getFloat("i", iDefault); + Kd = _preferences.getFloat("d", dDefault); + } + } +}; \ No newline at end of file diff --git a/Motor/BrushedMotor.cpp b/Motor/BrushedMotor.cpp new file mode 100644 index 0000000..f949957 --- /dev/null +++ b/Motor/BrushedMotor.cpp @@ -0,0 +1,22 @@ +#include "BrushedMotor.hpp" + +BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz, + uint32_t McpwmResolutionHz, int McpwmGroupId) {} + +void BrushedMotor::enable() {} + +void BrushedMotor::disable() {} + +void BrushedMotor::setDuty(uint16_t duty) {} + +uint16_t BrushedMotor::maxDuty() { + return 0; +} + +void BrushedMotor::forward() {} + +void BrushedMotor::reverse() {} + +void BrushedMotor::coast() {} + +void BrushedMotor::brake() {} \ No newline at end of file diff --git a/Motor/BrushedMotor.hpp b/Motor/BrushedMotor.hpp new file mode 100644 index 0000000..ecee0cd --- /dev/null +++ b/Motor/BrushedMotor.hpp @@ -0,0 +1,29 @@ +#include + +// TODO: implement class + +class BrushedMotor { + /* Driver for native MCPWM controller. */ + // TODO: define default values + public: + BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz, + uint32_t McpwmResolutionHz, int McpwmGroupId = 0); + void setDuty(uint16_t duty); + uint16_t maxDuty(); + void enable(); + void disable(); + void forward(); + void reverse(); + void coast(); + void brake(); + + protected: + /* MCPWM group */ + int _mcpwmGroupId = 0; + uint32_t _mcpwmResolutionHz; + + /* Brushed motor properties */ + uint32_t _pwmAGpioNum; + uint32_t _pwmBGpioNum; + uint32_t _pwmFreqHz; +}; \ No newline at end of file diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp index 9ee3263..b4b4a17 100644 --- a/RF/BluetoothDispatcher.cpp +++ b/RF/BluetoothDispatcher.cpp @@ -36,18 +36,23 @@ bool BluetoothDispatcher::initialize(bool loop_on_fail, int readTimeoutMS) { } } -void BluetoothDispatcher::tick(NewPackageCallback newPackageReceivedCb) { +void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageReceivedCb) { + _newPackageReceivedCb = newPackageReceivedCb; +} + +void BluetoothDispatcher::tick() { /* Call the callback, if new package received */ - while (_controller->available() and _buffer.length() <= _buffer_size) { _buffer += (char)_controller->read(); } + while (_controller->available() and _buffer.length() <= _buffer_size) { + _buffer += (char)_controller->read(); + } if (_buffer.endsWith("\r")) { char buffer[_buffer_size]; _buffer.substring(0, _buffer.lastIndexOf('\r'), buffer); ESP_LOGD(_tag, "Received new buffer %s", buffer); - if (newPackageReceivedCb) { - newPackageReceivedCb(buffer); + if (_newPackageReceivedCb) { + _newPackageReceivedCb(buffer); } _buffer.clear(); - _controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!")); } if (_buffer.length() > _available_buffer_size) { _buffer.clear(); @@ -64,7 +69,7 @@ void BluetoothDispatcher::_onConfirmRequest(uint16_t pin) { _controller->confirmReply(true); } -void BluetoothDispatcher::_onAuthComplete(boolean success) { +void BluetoothDispatcher::_onAuthComplete(boolean success) const { if (success) { ESP_LOGI(_tag, "Pairing success!"); } else { @@ -72,7 +77,7 @@ void BluetoothDispatcher::_onAuthComplete(boolean success) { } } -void BluetoothDispatcher::_onDeviceConnected(BTAddress device) { +void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const { ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str()); if (_deviceConnectedCallback) { _deviceConnectedCallback(device); @@ -88,8 +93,10 @@ void BluetoothDispatcher::sendPackage(const char *package, size_t size) { _controller->write((uint8_t *)package, size); } + 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) { + if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS + and deviceConnectedCallback) { deviceConnectedCallback(BTAddress(param->srv_open.rem_bda)); } } \ No newline at end of file diff --git a/RF/BluetoothDispatcher.hpp b/RF/BluetoothDispatcher.hpp index a99d740..aff5d47 100644 --- a/RF/BluetoothDispatcher.hpp +++ b/RF/BluetoothDispatcher.hpp @@ -26,22 +26,24 @@ class BluetoothDispatcher { public: BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter"); bool initialize(bool loop_on_fail = true, int readTimeoutMS = 2); - void tick(NewPackageCallback newPackageReceivedCb = nullptr); + void tick(); + void onNewPackageReceived(NewPackageCallback newPackageReceivedCb); void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb); void sendPackage(const char *package, size_t size); ~BluetoothDispatcher(); private: void _onConfirmRequest(uint16_t pin); - void _onAuthComplete(boolean success); - void _onDeviceConnected(BTAddress device); + void _onAuthComplete(boolean success) const; + void _onDeviceConnected(BTAddress device) const; const char *_device_name = nullptr; BluetoothSerial *_controller = nullptr; DeviceConnectedCb _deviceConnectedCallback = nullptr; + NewPackageCallback _newPackageReceivedCb = nullptr; constexpr static const char *_tag = "BluetoothDispatcher"; - static constexpr uint16_t _buffer_size = 512; - static constexpr uint16_t _available_buffer_size = 500; + static constexpr uint16_t _buffer_size = 522; + static constexpr uint16_t _available_buffer_size = 512; mString<_buffer_size> _buffer; }; \ No newline at end of file diff --git a/Sensors/Barometer.hpp b/Sensors/Barometer.hpp index f0f4337..d683050 100644 --- a/Sensors/Barometer.hpp +++ b/Sensors/Barometer.hpp @@ -1,5 +1,7 @@ #include "GyverBME280.h" +typedef std::function OnMeasuringFinishedCb; + class Barometer { public: Barometer(GyverBME280 *bme) { @@ -28,7 +30,7 @@ class Barometer { } void measureBaseAltitudeAsync(void) { - _isStartCalibration = true; + _isCalibration = true; } float altitude() /* [cm] */ { @@ -39,14 +41,21 @@ class Barometer { return altitude() - _startedAltitude; } + void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) { + _measuaringHeightFinishedCb = callback; + } + void tick() { - if (_isStartCalibration) { - if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) { + if (_isCalibration) { + if (millis() - _calibrationTimer >= _calibrationIterationDelay) { _startedAltitude += altitude(); } if (++_calibrationIterationsCounter >= _calibrationIterationsCount) { _startedAltitude /= _calibrationIterationsCount; - _isStartCalibration = false; + _isCalibration = false; + if (_measuaringHeightFinishedCb) { + _measuaringHeightFinishedCb(); + } } } } @@ -54,9 +63,10 @@ class Barometer { private: GyverBME280 *_bme; float _startedAltitude = 0; - bool _isStartCalibration = false; - int _calibrationTimer; + bool _isCalibration = false; + uint32_t _calibrationTimer; int _calibrationIterationsCounter = 0; + OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr; static constexpr int _calibrationIterationsCount = 1500; static constexpr int _calibrationIterationDelay = 1; // [ms] }; \ No newline at end of file diff --git a/Sensors/MPU.hpp b/Sensors/MPU.hpp index f6fb214..e124402 100644 --- a/Sensors/MPU.hpp +++ b/Sensors/MPU.hpp @@ -1,4 +1,5 @@ #include "MPU6050_6Axis_MotionApps20.h" +#include "Preferences.h" #include "board_pins.h" #include "esp_log.h" @@ -7,6 +8,8 @@ static void IRAM_ATTR _dmpInterruption() { _isDMPDataReady = true; } +typedef std::function OnCalibrationFinishedCb; + class MPU { public: MPU(MPU6050_6Axis_MotionApps20 *mpu) { @@ -20,6 +23,13 @@ class MPU { bool initialize() { _mpu->initialize(); + + if (!_preferences.begin("imu")) { + ESP_LOGE(_TAG, "Failed to open the preferences!"); + } else { + _loadOffsets(); + } + delay(250); if (!_mpu->testConnection()) { ESP_LOGE(_TAG, "MPU6050 test connection failed!"); @@ -52,12 +62,14 @@ class MPU { Quaternion q; VectorFloat gravity; VectorInt16 accel; + VectorInt16 gyro; VectorInt16 accelReal; _mpu->dmpGetQuaternion(&q, _fifoBuffer); _mpu->dmpGetGravity(&gravity, &q); _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity); _mpu->dmpGetAccel(&accel, _fifoBuffer); + _mpu->dmpGetGyro(&gyro, _fifoBuffer); _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity); _isDMPDataReady = false; @@ -65,51 +77,121 @@ 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; + _gx = gyro.x / 131.f; + _gy = gyro.y / 131.f; + _gz = gyro.z / 131.f; _calculateZInertial(gravity); + + if (_isCalibration) { + if (_calibrationsIterCounter >= _calibrationIterCount) { + _axOffset /= _calibrationsIterCounter; + _ayOffset /= _calibrationsIterCounter; + _azOffset /= _calibrationsIterCounter; + + _gxOffset /= _calibrationsIterCounter; + _gyOffset /= _calibrationsIterCounter; + _gzOffset /= _calibrationsIterCounter; + + _prOffset[0] /= _calibrationsIterCounter; + _prOffset[1] /= _calibrationsIterCounter; + + _isCalibration = false; + _updateOffsets(); + if (_calibrationFinishedCb) { + _calibrationFinishedCb(); + } + } else { + _axOffset += _ax; + _ayOffset += _ay; + _azOffset += _az; + _gxOffset += _gx; + _gyOffset += _gy; + _gzOffset += _gz; + _prOffset[0] += _ypr[1]; + _prOffset[1] += _ypr[2]; + } + } + return true; } /* getters */ - float accZInertial() { + float accZInertial() const { return _AccZInertial; } - float yaw() { + float yaw() const { return degrees(_ypr[0]); - }; - float pitch() { - return degrees(_ypr[1]); - }; - float roll() { - return degrees(_ypr[2]); - }; - - float ax() { - return _ax; } - float ay() { - return _ay; + float pitch() const { + if (_isCalibration) { + return degrees(_ypr[1]); + } else { + return degrees(_ypr[1] - _yprOffset[1]); + } } - float az() { - return _az; + float roll() const { + if (_isCalibration) { + return degrees(_ypr[2]); + } else { + return degrees(_ypr[2] - _yprOffset[2]); + } } - float gravityZ() { + float ax() const { + if (_isCalibration) { + return _ax; + } else { + return _ax - _axOffset; + } + } + float ay() const { + if (_isCalibration) { + return _ay; + } else { + return _ay - _ayOffset; + } + } + float az() const { + if (_isCalibration) { + return _az; + } else { + return _az - _azOffset; + } + } + + float gravityZ() const { return _gravity; } + float gx() const { + if (_isCalibration) { + return _gx; + } else { + return _gx - _gxOffset; + } + } + float gy() const { + if (_isCalibration) { + return _gy; + } else { + return _gy - _gyOffset; + } + } + float gz() const { + if (_isCalibration) { + return _gz; + } else { + return _gz - _gzOffset; + } + } - float gx() { - return _gx; + void startCalibration() { + _isCalibration = true; } - float gy() { - return _gy; - } - float gz() { - return _gz; + + void onCalibrationFinished(OnCalibrationFinishedCb callback) { + _calibrationFinishedCb = callback; } private: @@ -119,16 +201,50 @@ class MPU { float _gx, _gy, _gz; float _gravity; float _AccZInertial = 0; + + float _axOffset = 0, _ayOffset = 0, _azOffset = 0; + float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0; + float _prOffset[2]; // yaw isn't used + uint8_t _fifoBuffer[45]; + Preferences _preferences; const char *_TAG = "MPU6050 module"; + static constexpr const int _calibrationIterCount = 250; + + bool _isCalibration = false; + int _calibrationsIterCounter = 0; + OnCalibrationFinishedCb _calibrationFinishedCb = nullptr; 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; + _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + + cos(anglePitch) * cos(angleRoll) * _az; + _AccZInertial = (_AccZInertial - 1) * gravity.z * 100.f; + } + + void _loadOffsets() { + _axOffset = _preferences.getFloat("ax", 0.f); + _ayOffset = _preferences.getFloat("ax", 0.f); + _azOffset = _preferences.getFloat("az", 0.f); + _gxOffset = _preferences.getFloat("gx", 0.f); + _gyOffset = _preferences.getFloat("gy", 0.f); + _gzOffset = _preferences.getFloat("gz", 0.f); + _ypr[1] = _preferences.getFloat("pitch", 0.f); + _ypr[2] = _preferences.getFloat("roll", 0.f); + } + + void _updateOffsets() { + _preferences.putFloat("ax", _axOffset); + _preferences.putFloat("ay", _ayOffset); + _preferences.putFloat("az", _azOffset); + _preferences.putFloat("gx", _gxOffset); + _preferences.putFloat("gy", _gyOffset); + _preferences.putFloat("gz", _gzOffset); + _preferences.putFloat("pitch", _prOffset[0]); + _preferences.putFloat("roll", _prOffset[1]); } }; diff --git a/Sensors/Sensors.cpp b/Sensors/Sensors.cpp index d1b7765..3de981b 100644 --- a/Sensors/Sensors.cpp +++ b/Sensors/Sensors.cpp @@ -1,10 +1,15 @@ #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; +Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail) { + assert(barometer != nullptr); + assert(mpu != nullptr); + assert(filter != nullptr); + assert(battery != nullptr); + + _barometer = barometer; + _mpu = mpu; + _2d_filter = filter; + _battery = battery; ESP_LOGI(_tag, "Initialize BMP280..."); if (_barometer->initialize()) { @@ -44,12 +49,12 @@ void Sensors::measureBaseAltitudeAsync(void) { _barometer->measureBaseAltitudeAsync(); } -float Sensors::rawFlightHeight(void) { +float Sensors::rawFlightHeight(void) const { /* Returns flight height from barometer immediatly */ return _barometer->flightHeight(); } -float Sensors::flightHeight(void) { +float Sensors::flightHeight(void) const { /* Returns flight height from cache immediatly */ if (_flightHeightFromBarometer == NAN) { ESP_LOGW(_tag, "flightHeight called, but tick() has called never"); @@ -59,8 +64,8 @@ float Sensors::flightHeight(void) { } } -MpuData Sensors::mpuData() { - if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) { +MpuData Sensors::mpuData() const { + if (_mpuData.ax == NAN and _mpuData.gravity == NAN) { ESP_LOGW(_tag, "MPU data looks like .tick() has never called"); } return _mpuData; @@ -69,6 +74,7 @@ MpuData Sensors::mpuData() { bool Sensors::tick(void) { /* Super loop iteraion */ /* Return true on update */ + _barometer->tick(); bool err; bool isMpuDataReady = _mpu->tick(err); if (isMpuDataReady and !err) { @@ -82,7 +88,8 @@ bool Sensors::tick(void) { .yaw = _mpu->yaw(), .pitch = _mpu->pitch(), .roll = _mpu->roll(), - .gravityZ = _mpu->gravityZ() }; + .gravity = _mpu->gravityZ(), + .zInertial = _mpu->accZInertial() }; return true; } else if (err) { ESP_LOGE(_tag, "Failed to get DMP data!"); @@ -90,6 +97,18 @@ bool Sensors::tick(void) { return false; } -int Sensors::batteryCharge(void) { +int Sensors::batteryCharge(void) const { return _battery->percent(); +} + +void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) { + _mpu->onCalibrationFinished(callback); +} + +void Sensors::startMpuCalibration() { + _mpu->startCalibration(); +} + +void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) { + _barometer->onMeasuaringHeightFinished(callback); } \ No newline at end of file diff --git a/Sensors/Sensors.hpp b/Sensors/Sensors.hpp index f41b8fe..0edad5a 100644 --- a/Sensors/Sensors.hpp +++ b/Sensors/Sensors.hpp @@ -10,23 +10,28 @@ struct MpuData { float ax, ay, az; float gx, gy, gz; float yaw, pitch, roll; - float gravityZ; + float gravity; + float zInertial; }; class Sensors { public: - Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail = true); + 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); + void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback); + float rawFlightHeight(void) const; + float flightHeight(void) const; - MpuData mpuData(void); + void startMpuCalibration(); + void onMpuCalibrationFinished(OnCalibrationFinishedCb callback); - int batteryCharge(void); // [%] + MpuData mpuData(void) const; + + int batteryCharge(void) const; // [%] bool tick(void); @@ -39,7 +44,7 @@ class Sensors { /* cached filtered values */ float _flightHeightFromBarometer = NAN; float _zVelocityAltitude = NAN; - MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; + MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; static constexpr const char *_tag = "Sensors"; diff --git a/main.cpp b/main.cpp index f12e0cb..95821a4 100644 --- a/main.cpp +++ b/main.cpp @@ -1,54 +1,36 @@ -#include "Filters/Kalman2DFilter.hpp" -#include "GyverBME280.h" -#include "I2Cdev.h" -#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 +#include "App.hpp" +#include "BoardI2C.hpp" +//#include "MPU6050_6Axis_MotionApps20.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; +BoardI2C i2c; +/* BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial()); +Sensors *sensors = nullptr; */ + +static Application *app = nullptr; void setup() { Serial.begin(115200); - Serial.setDebugOutput(true); - - 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"); + app = new Application(new FlightDispatcher( + new BluetoothDispatcher(new BluetoothSerial()), + new FlightController( + new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), + new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()), + new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1), + new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2), + new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3)))); + /*sensors = new Sensors( + new Barometer(new GyverBME280(i2c)), new MPU(new + MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f), + new BatteryController()); sensors->measureBaseAltitudeSync(); + bluetoothDispatcher.initialize(); + Serial.println("System initialized");*/ } void loop() { - bool err; - barometer.tick(); - if (mpu.tick(err) && !err) { - float kalmanAltitude, ZVelocityAltitude; - float barometerAltitude = barometer.flightHeight(); - kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude); - //Serial.print(barometerAltitude); - } - bluetoothDispatcher.tick(); - delay(1); + app->tick(); + /* + sensors->tick(); + bluetoothDispatcher.tick(); + delay(1);*/ } \ No newline at end of file