From 0621715ce569d070e9266b111e2a5c1765c460ce Mon Sep 17 00:00:00 2001 From: gogacoder Date: Tue, 7 Nov 2023 21:37:23 +0700 Subject: [PATCH] Initial commit --- .clang-format | 190 +++++++++++++++++++++++++++++++++++++ Barometer.h | 83 ++++++++++++++++ BatteryController.h | 19 ++++ GyverBME280.cpp | 219 +++++++++++++++++++++++++++++++++++++++++++ GyverBME280.h | 112 ++++++++++++++++++++++ Kalman2DFilter.h | 45 +++++++++ MPU.h | 94 +++++++++++++++++++ arduino-firmware.ino | 92 ++++++++++++++++++ board_pins.h | 8 ++ 9 files changed, 862 insertions(+) create mode 100644 .clang-format create mode 100644 Barometer.h create mode 100644 BatteryController.h create mode 100644 GyverBME280.cpp create mode 100644 GyverBME280.h create mode 100644 Kalman2DFilter.h create mode 100644 MPU.h create mode 100644 arduino-firmware.ino create mode 100644 board_pins.h diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..e608f4e --- /dev/null +++ b/.clang-format @@ -0,0 +1,190 @@ +# Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration +--- +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: None +AlignConsecutiveBitFields: None +AlignConsecutiveDeclarations: None +AlignConsecutiveMacros: None +AlignEscapedNewlines: DontAlign +AlignOperands: Align +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Always +AllowShortCaseLabelsOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Empty +AllowShortIfStatementsOnASingleLine: AllIfsAndElse +AllowShortLambdasOnASingleLine: Empty +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: No +AttributeMacros: + - __capability +BasedOnStyle: LLVM +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAfterJavaFieldAnnotations: false +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeBraces: Attach +BreakBeforeConceptDeclarations: false +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakConstructorInitializersBeforeComma: false +BreakInheritanceList: BeforeColon +BreakStringLiterals: false +ColumnLimit: 0 +CommentPragmas: '' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: false +DeriveLineEnding: true +DerivePointerAlignment: true +DisableFormat: false +EmptyLineAfterAccessModifier: Leave +EmptyLineBeforeAccessModifier: Leave +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: false +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: true +IndentCaseLabels: true +IndentExternBlock: Indent +IndentGotoLabels: false +IndentPPDirectives: None +IndentRequires: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertTrailingCommas: None +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +LambdaBodyIndentation: Signature +Language: Cpp +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 100000 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 1 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 1 +PenaltyBreakFirstLessLess: 1 +PenaltyBreakOpenParenthesis: 1 +PenaltyBreakString: 1 +PenaltyBreakTemplateDeclaration: 1 +PenaltyExcessCharacter: 1 +PenaltyIndentedWhitespace: 1 +PenaltyReturnTypeOnItsOwnLine: 1 +PointerAlignment: Right +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: false +RemoveBracesLLVM: false +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 0 +SortIncludes: Never +SortJavaStaticImport: Before +SortUsingDeclarations: false +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Leave +SpacesInCStyleCastParentheses: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: false +SpacesInLineCommentPrefix: + Minimum: 0 + Maximum: -1 +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 2 +UseCRLF: false +UseTab: Never +WhitespaceSensitiveMacros: + - STRINGIZE + - PP_STRINGIZE + - BOOST_PP_STRINGIZE + - NS_SWIFT_NAME + - CF_SWIFT_NAME diff --git a/Barometer.h b/Barometer.h new file mode 100644 index 0000000..94b0cd1 --- /dev/null +++ b/Barometer.h @@ -0,0 +1,83 @@ +#include "GyverBME280.h" + +class Barometer { +public: + Barometer(GyverBME280 *bme) { + _bme = bme; + }; + + ~Barometer() { + delete this->_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); + } + _startedAltitude /= _calibrationIterationsCount; + } + + void measureBaseAltitudeAsync(void) { + _isStartCalibration = true; + } + + 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 = nullptr; + 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 new file mode 100644 index 0000000..68e4411 --- /dev/null +++ b/BatteryController.h @@ -0,0 +1,19 @@ +#include "board_pins.h" + +class BatteryController { +public: + BatteryController(){} + + 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 diff --git a/GyverBME280.cpp b/GyverBME280.cpp new file mode 100644 index 0000000..a5b38b5 --- /dev/null +++ b/GyverBME280.cpp @@ -0,0 +1,219 @@ +#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 new file mode 100644 index 0000000..a88251e --- /dev/null +++ b/GyverBME280.h @@ -0,0 +1,112 @@ +/* + Лёгкая библиотека для работы с 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 new file mode 100644 index 0000000..8630515 --- /dev/null +++ b/Kalman2DFilter.h @@ -0,0 +1,45 @@ +#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 new file mode 100644 index 0000000..5b92039 --- /dev/null +++ b/MPU.h @@ -0,0 +1,94 @@ +#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/arduino-firmware.ino b/arduino-firmware.ino new file mode 100644 index 0000000..2e2aa46 --- /dev/null +++ b/arduino-firmware.ino @@ -0,0 +1,92 @@ +#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/board_pins.h b/board_pins.h new file mode 100644 index 0000000..20ee6f7 --- /dev/null +++ b/board_pins.h @@ -0,0 +1,8 @@ +#include "driver/adc.h" + +#define I2C_SDA_PIN 21 +#define I2C_SCL_PIN 18 +#define MPU6050_INT_PIN 19 +#define BATTERY_ADC_CHANNEL ADC2_CHANNEL_0 +#define BATTERY_DATA_PIN 4 +#define BATTERY_DATA_SWITCH_PIN 17 \ No newline at end of file