Helicopter/firmware
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Goga Coder
2023-11-07 21:37:23 +07:00
parent fd6d5b88b1
commit 0621715ce5
9 changed files with 862 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
.clang-format Normal file
View File

@@ -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

83
Barometer.h Normal file
View File

@@ -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);
}
};

19
BatteryController.h Normal file
View File

@@ -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);
}
};

219
GyverBME280.cpp Normal file
View File

@@ -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();
}

112
GyverBME280.h Normal file
View File

@@ -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 <Arduino.h>
#include <Wire.h>
#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

45
Kalman2DFilter.h Normal file
View File

@@ -0,0 +1,45 @@
#include <BasicLinearAlgebra.h>
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;
};

94
MPU.h Normal file
View File

@@ -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;
}
};

92
arduino-firmware.ino Normal file
View File

@@ -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);
}*/
}

8
board_pins.h Normal file
View File

@@ -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