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

BluetoothDispatcher was added & reformatting

Browse Source
This commit is contained in:
Goga Coder
2024-01-05 20:38:10 +07:00
parent 02f53416ab
commit 21dbd883f7
15 changed files with 522 additions and 647 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
Barometer.h
View File

@@ -1,83 +1,84 @@
#include "GyverBME280.h"
class Barometer {
public:
Barometer(GyverBME280 *bme) {
_bme = bme;
};
~Barometer() {
delete this->_bme;
}
public:
Barometer(GyverBME280 *bme) {
_bme = bme;
};
bool initialize(void) {
_bme->setMode(NORMAL_MODE);
_bme->setFilter(FILTER_COEF_16);
_bme->setTempOversampling(OVERSAMPLING_2);
_bme->setPressOversampling(OVERSAMPLING_16);
_bme->setStandbyTime(STANDBY_500US);
return _bme->begin();
}
void measureBaseAltitudeSync(void) {
for(int i = 0; i < _calibrationIterationsCount; ++i){
_startedAltitude += altitude();
delay(1);
~Barometer() {
delete this->_bme;
}
_startedAltitude /= _calibrationIterationsCount;
}
void measureBaseAltitudeAsync(void) {
_isStartCalibration = true;
}
bool initialize(void) {
_bme->setMode(NORMAL_MODE);
_bme->setFilter(FILTER_COEF_16);
_bme->setTempOversampling(OVERSAMPLING_2);
_bme->setPressOversampling(OVERSAMPLING_16);
_bme->setStandbyTime(STANDBY_500US);
return _bme->begin();
}
float rawAltitude() {
return pressureToAltitude(_bme->readPressure())*100;
}
float rawFlightHeight() {
return rawAltitude() - _startedAltitude;
}
float altitude() {
return _filterRA(pressureToAltitude(_bme->readPressure())*100); // convert from m to cm
}
float flightHeight() {
return altitude() - _startedAltitude;
}
void tick() {
if(_isStartCalibration) {
if(_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
_startedAltitude += altitude();
}
if(++_calibrationIterationsCounter >= _calibrationIterationsCount) {
void measureBaseAltitudeSync(void) {
for (int i = 0; i < _calibrationIterationsCount; ++i) {
_startedAltitude += altitude();
delay(1);
}
_startedAltitude /= _calibrationIterationsCount;
_isStartCalibration = false;
}
}
}
private:
GyverBME280 *_bme = nullptr;
float _startedAltitude = 0;
bool _isStartCalibration = false;
int _calibrationTimer;
int _calibrationIterationsCounter = 0;
const int _calibrationIterationsCount = 1500;
const int _calibrationIterationDelay = 1; // [ms]
void measureBaseAltitudeAsync(void) {
_isStartCalibration = true;
}
float _filterRA(float newVal) {
// running average filter
constexpr auto WINDOW_SIZE = 100;
static int t = 0;
static float vals[WINDOW_SIZE];
static float average = 0;
if (++t >= WINDOW_SIZE) t = 0;
average -= vals[t];
average += newVal;
vals[t] = newVal;
return ((float)average / WINDOW_SIZE);
}
float rawAltitude() {
return pressureToAltitude(_bme->readPressure()) * 100;
}
float rawFlightHeight() {
return rawAltitude() - _startedAltitude;
}
float altitude() {
return _filterRA(pressureToAltitude(_bme->readPressure()) * 100); // convert from m to cm
}
float flightHeight() {
return altitude() - _startedAltitude;
}
void tick() {
if (_isStartCalibration) {
if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
_startedAltitude += altitude();
}
if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
_startedAltitude /= _calibrationIterationsCount;
_isStartCalibration = false;
}
}
}
private:
GyverBME280 *_bme;
float _startedAltitude = 0;
bool _isStartCalibration = false;
int _calibrationTimer;
int _calibrationIterationsCounter = 0;
const int _calibrationIterationsCount = 1500;
const int _calibrationIterationDelay = 1; // [ms]
float _filterRA(float newVal) {
// running average filter
constexpr auto WINDOW_SIZE = 100;
static int t = 0;
static float vals[WINDOW_SIZE];
static float average = 0;
if (++t >= WINDOW_SIZE)
t = 0;
average -= vals[t];
average += newVal;
vals[t] = newVal;
return ((float)average / WINDOW_SIZE);
}
};

28
BatteryController.h
View File

@@ -1,19 +1,19 @@
#include "board_pins.h"
class BatteryController {
public:
BatteryController(){}
public:
BatteryController() {}
void initialize() {
pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
}
void initialize() {
pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
}
float measureVoltage() {
return analogRead(BATTERY_DATA_PIN)*3.3/4095;
}
int percent(int minVoltage=7200, int maxVoltage=8400) {
return map(int(measureVoltage()*1000), minVoltage, maxVoltage, 0, 100);
}
};
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);
}
};

51
Filters/Kalman2DFilter.hpp Normal file
View File

@@ -0,0 +1,51 @@
#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;
};

46
Filters/kalman.hpp Normal file
View File

@@ -0,0 +1,46 @@
// упрощённый Калман для одномерного случая
#ifndef _GKalman_h
#define _GKalman_h
class GKalman {
public:
// разброс измерения, разброс оценки, скорость изменения значений
GKalman(float mea_e, float est_e, float q) {
setParameters(mea_e, est_e, q);
}
// разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
GKalman(float mea_e, float q) {
setParameters(mea_e, mea_e, q);
}
// разброс измерения, разброс оценки, скорость изменения значений
void setParameters(float mea_e, float est_e, float q) {
_err_measure = mea_e;
_err_estimate = est_e;
_q = q;
}
// разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
void setParameters(float mea_e, float q) {
setParameters(mea_e, mea_e, q);
}
// возвращает фильтрованное значение
float filtered(float value) {
float _kalman_gain, _current_estimate;
_kalman_gain = _err_estimate / (_err_estimate + _err_measure);
_current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
_err_estimate = (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
_last_estimate=_current_estimate;
return _current_estimate;
}
private:
float _err_measure = 0.0;
float _err_estimate = 0.0;
float _q = 0.0;
float _last_estimate = 0.0;
};
#endif

19
Filters/median3.hpp Normal file
View File

@@ -0,0 +1,19 @@
// быстрый медианный фильтр 3-го порядка
#ifndef _GMedian3_h
#define _GMedian3_h
template < typename TYPE >
class GMedian3 {
public:
TYPE filtered(TYPE value) { // возвращает фильтрованное значение
buf[_counter] = value;
if (++_counter > 2) _counter = 0;
return (max(buf[0], buf[1]) == max(buf[1], buf[2])) ? max(buf[0], buf[2]) : max(buf[1], min(buf[0], buf[2]));
}
private:
TYPE buf[3];
uint8_t _counter = 0;
};
#endif

49
Filters/runningAverage.hpp Normal file
View File

@@ -0,0 +1,49 @@
// экспоненциальное бегущее среднее
#ifndef _GFilterRA_h
#define _GFilterRA_h
class GFilterRA {
public:
GFilterRA(){}
GFilterRA(float coef, uint16_t interval) {
_coef = coef;
_prd = interval;
}
GFilterRA(float coef) {
_coef = coef;
}
void setCoef(float coef) {
_coef = coef;
}
void setPeriod(uint16_t interval) {
_prd = interval;
}
float filteredTime(float value) {
if (millis() - _tmr >= _prd) {
_tmr += _prd;
filtered(value);
}
return _fil;
}
float filtered(float value) {
return _fil += (value - _fil) * _coef;
}
//
void setStep(uint16_t interval) {
_prd = interval;
}
private:
float _coef = 0.0, _fil = 0.0;
uint32_t _tmr = 0;
uint16_t _prd = 0;
};
#endif

219
GyverBME280.cpp
View File

@@ -1,219 +0,0 @@
#include "GyverBME280.h"
/* ============ Utilities ============ */
float pressureToAltitude(float pressure) {
if (!pressure) return 0; // If the pressure module has been disabled return '0'
pressure /= 100.0F; // Convert [Pa] to [hPa]
return 44330.0f * (1.0f - pow(pressure / 1013.25f, 0.1903f)); // Сalculate altitude
}
float pressureToMmHg(float pressure) {
return (float)(pressure * 0.00750061683f); // Convert [Pa] to [mm Hg]
}
/* ============ Setup & begin ============ */
GyverBME280::GyverBME280(TwoWire &wire) {
_wire = &wire;
}
bool GyverBME280::begin() {
return begin(0x76);
}
bool GyverBME280::begin(uint8_t address) {
_i2c_address = address;
/* === Start I2C bus & check BME280 === */
if (!reset()) return false; // BME280 software reset & ack check
uint8_t ID = readRegister(0xD0);
if (ID != 0x60 && ID != 0x58) return false; // Check chip ID (bme/bmp280)
readCalibrationData(); // Read all calibration values
/* === Load settings to BME280 === */
writeRegister(0xF2, _hum_oversampl); // write hum oversampling value
writeRegister(0xF2, readRegister(0xF2)); // rewrite hum oversampling register
writeRegister(0xF4, ((_temp_oversampl << 5) | (_press_oversampl << 2) | _operating_mode)); // write temp & press oversampling value , normal mode
writeRegister(0xF5, ((_standby_time << 5) | (_filter_coef << 2))); // write standby time & filter coef
return true;
}
void GyverBME280::setMode(uint8_t mode) {
_operating_mode = mode;
}
void GyverBME280::setFilter(uint8_t mode) {
_filter_coef = mode;
}
void GyverBME280::setStandbyTime(uint8_t mode) {
_standby_time = mode;
}
void GyverBME280::setHumOversampling(uint8_t mode) {
_hum_oversampl = mode;
}
void GyverBME280::setTempOversampling(uint8_t mode) {
_temp_oversampl = mode;
}
void GyverBME280::setPressOversampling(uint8_t mode) {
_press_oversampl = mode;
}
/* ============ Reading ============ */
int32_t GyverBME280::readTempInt(void) {
int32_t temp_raw = readRegister24(0xFA); // Read 24-bit value
if (temp_raw == 0x800000) return 0; // If the temperature module has been disabled return '0'
temp_raw >>= 4; // Start temperature reading in integers
int32_t value_1 = ((((temp_raw >> 3) - ((int32_t)CalibrationData._T1 << 1))) *
((int32_t)CalibrationData._T2)) >> 11;
int32_t value_2 = (((((temp_raw >> 4) - ((int32_t)CalibrationData._T1)) *
((temp_raw >> 4) - ((int32_t)CalibrationData._T1))) >> 12) * ((int32_t)CalibrationData._T3)) >> 14;
return ((int32_t)value_1 + value_2); // Return temperature in integers
}
float GyverBME280::readTemperature(void) {
int32_t temp_raw = readTempInt();
float T = (temp_raw * 5 + 128) >> 8;
return T / 100.0; // Return temperature in float
}
float GyverBME280::readPressure(void) {
uint32_t press_raw = readRegister24(0xF7); // Read 24-bit value
if (press_raw == 0x800000) return 0; // If the pressure module has been disabled return '0'
press_raw >>= 4; // Start pressure converting
int64_t value_1 = ((int64_t)readTempInt()) - 128000;
int64_t value_2 = value_1 * value_1 * (int64_t)CalibrationData._P6;
value_2 = value_2 + ((value_1 * (int64_t)CalibrationData._P5) << 17);
value_2 = value_2 + (((int64_t)CalibrationData._P4) << 35);
value_1 = ((value_1 * value_1 * (int64_t)CalibrationData._P3) >> 8) + ((value_1 * (int64_t)CalibrationData._P2) << 12);
value_1 = (((((int64_t)1) << 47) + value_1)) * ((int64_t)CalibrationData._P1) >> 33;
if (!value_1) return 0; // Avoid division by zero
int64_t p = 1048576 - press_raw;
p = (((p << 31) - value_2) * 3125) / value_1;
value_1 = (((int64_t)CalibrationData._P9) * (p >> 13) * (p >> 13)) >> 25;
value_2 = (((int64_t)CalibrationData._P8) * p) >> 19;
p = ((p + value_1 + value_2) >> 8) + (((int64_t)CalibrationData._P7) << 4);
return (float)p / 256; // Return pressure in float
}
float GyverBME280::readHumidity(void) {
_wire->beginTransmission(_i2c_address); // Start I2C transmission
_wire->write(0xFD); // Request humidity data register
if (_wire->endTransmission() != 0) return 0;
_wire->requestFrom(_i2c_address, 2); // Request humidity data
int32_t hum_raw = ((uint16_t)_wire->read() << 8) | (uint16_t)_wire->read(); // Read humidity data
if (hum_raw == 0x8000) return 0; // If the humidity module has been disabled return '0'
int32_t value = (readTempInt() - ((int32_t)76800)); // Start humidity converting
value = (((((hum_raw << 14) - (((int32_t)CalibrationData._H4) << 20) -
(((int32_t)CalibrationData._H5) * value)) +((int32_t)16384)) >> 15) *
(((((((value * ((int32_t)CalibrationData._H6)) >> 10) *(((value *
((int32_t)CalibrationData._H3)) >> 11) + ((int32_t)32768))) >> 10) +
((int32_t)2097152)) * ((int32_t)CalibrationData._H2) + 8192) >> 14));
value = (value - (((((value >> 15) * (value >> 15)) >> 7) * ((int32_t)CalibrationData._H1)) >> 4));
value = (value < 0) ? 0 : value;
value = (value > 419430400) ? 419430400 : value;
float h = (value >> 12);
return h / 1024.0; // Return humidity in float
}
/* ============ Misc ============ */
bool GyverBME280::isMeasuring(void) {
return (bool)((readRegister(0xF3) & 0x08) >> 3); // Read status register & mask bit "measuring"
}
void GyverBME280::oneMeasurement(void) {
writeRegister(0xF4 , ((readRegister(0xF4) & 0xFC) | 0x02)); // Set the operating mode to FORCED_MODE
}
/* ============ Private ============ */
/* = BME280 software reset = */
bool GyverBME280::reset(void) {
if (!writeRegister(0x0E , 0xB6)) return false;
delay(10);
return true;
}
/* = Read and combine three BME280 registers = */
uint32_t GyverBME280::readRegister24(uint8_t address) {
_wire->beginTransmission(_i2c_address);
_wire->write(address);
if (_wire->endTransmission() != 0) return 0x800000;
_wire->requestFrom(_i2c_address, 3);
return (((uint32_t)_wire->read() << 16) | ((uint32_t)_wire->read() << 8) | (uint32_t)_wire->read());
}
/* = Write one 8-bit BME280 register = */
bool GyverBME280::writeRegister(uint8_t address , uint8_t data) {
_wire->beginTransmission(_i2c_address);
_wire->write(address);
_wire->write(data);
if (_wire->endTransmission() != 0) return false;
return true;
}
/* = Read one 8-bit BME280 register = */
uint8_t GyverBME280::readRegister(uint8_t address) {
_wire->beginTransmission(_i2c_address);
_wire->write(address);
if (_wire->endTransmission() != 0) return 0;
_wire->requestFrom(_i2c_address , 1);
return _wire->read();
}
/* = Structure to store all calibration values = */
void GyverBME280::readCalibrationData(void) {
/* first part request*/
_wire->beginTransmission(_i2c_address);
_wire->write(0x88);
if (_wire->endTransmission() != 0) return;
_wire->requestFrom(_i2c_address , 25);
/* reading */
CalibrationData._T1 = (_wire->read() | (_wire->read() << 8));
CalibrationData._T2 = (_wire->read() | (_wire->read() << 8));
CalibrationData._T3 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P1 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P2 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P3 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P4 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P5 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P6 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P7 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P8 = (_wire->read() | (_wire->read() << 8));
CalibrationData._P9 = (_wire->read() | (_wire->read() << 8));
CalibrationData._H1 = _wire->read();
/* second part request*/
_wire->beginTransmission(_i2c_address);
_wire->write(0xE1);
_wire->endTransmission();
_wire->requestFrom(_i2c_address , 8);
/* reading */
CalibrationData._H2 = (_wire->read() | (_wire->read() << 8));
CalibrationData._H3 = _wire->read();
CalibrationData._H4 = (_wire->read() << 4);
uint8_t interVal = _wire->read();
CalibrationData._H4 |= (interVal & 0xF);
CalibrationData._H5 = (((interVal & 0xF0) >> 4) | (_wire->read() << 4));
CalibrationData._H6 = _wire->read();
}

112
GyverBME280.h
View File

@@ -1,112 +0,0 @@
/*
Лёгкая библиотека для работы с BME280 по I2C для Arduino
Документация:
GitHub: https://github.com/GyverLibs/GyverBME280
Egor 'Nich1con' Zaharov for AlexGyver, alex@alexgyver.ru
https://alexgyver.ru/
MIT License
Версии:
v1.3 - исправлена ошибка при отриц. температуре
v1.4 - разбил на h и cpp
v1.5 - добавлена поддержка BMP280s
*/
#ifndef GyverBME280_h
#define GyverBME280_h
#include <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
View File

@@ -1,45 +0,0 @@
#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
View File

@@ -1,94 +0,0 @@
#include "MPU6050_6Axis_MotionApps20.h"
#include "board_pins.h"
#include "esp_log.h"
volatile bool _isDMPDataReady = false;
void IRAM_ATTR _dmpInterruption() {_isDMPDataReady = true;}
class MPU {
public:
MPU(MPU6050_6Axis_MotionApps20 *mpu) {
_mpu = mpu;
}
~MPU() {
delete _mpu;
}
bool initialize() {
_mpu->initialize();
delay(250);
if(!_mpu->testConnection()) {
ESP_LOGE(_TAG, "MPU6050 test connection failed!");
return false;
}
_mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
if(_mpu->dmpInitialize()) {
ESP_LOGE(_TAG, "Failed to initialize DMP!");
return false;
}
_mpu->setDMPEnabled(true);
attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING);
return true;
}
bool tick(bool &err) {
err = false;
if(!_isDMPDataReady) {
return false;
}
if(!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) {
ESP_LOGE(_TAG, "Failed to get DMP data!");
err = true;
return false;
}
Quaternion q;
VectorFloat gravity;
VectorInt16 accel;
VectorInt16 accelReal;
_mpu->dmpGetQuaternion(&q, _fifoBuffer);
_mpu->dmpGetGravity(&gravity, &q);
_mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
_mpu->dmpGetAccel(&accel, _fifoBuffer);
_mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
_isDMPDataReady = false;
_ax = accel.x/8192.f;
_ay = accel.y/8192.f;
_az = accel.z/8192.f;
_calculateZInertial(gravity);
return true;
}
/* getters */
float accZInertial() {return _AccZInertial;}
float yaw() {return degrees(_ypr[0]);};
float pitch() {return degrees(_ypr[1]);};
float roll() {return degrees(_ypr[2]);};
private:
MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
float _ypr[3];
float _ax, _ay, _az;
float _AccZInertial = 0;
uint8_t _fifoBuffer[45];
//volatile bool _isDMPDataReady = false;
const char *_TAG = "MPU6050 module";
void _calculateZInertial(VectorFloat &gravity) {
const float anglePitch = _ypr[1];
const float angleRoll = _ypr[2];
_AccZInertial = -sin(anglePitch)*_ax +
cos(anglePitch)*
sin(angleRoll)*_ay +
cos(anglePitch)*
cos(angleRoll)*_az;
_AccZInertial = (_AccZInertial-1)*gravity.z*100;
}
};

100
MPU.hpp Normal file
View File

@@ -0,0 +1,100 @@
#include "MPU6050_6Axis_MotionApps20.h"
#include "board_pins.h"
#include "esp_log.h"
volatile bool _isDMPDataReady = false;
void IRAM_ATTR _dmpInterruption() {
_isDMPDataReady = true;
}
class MPU {
public:
MPU(MPU6050_6Axis_MotionApps20 *mpu) {
_mpu = mpu;
}
~MPU() {
delete _mpu;
}
bool initialize() {
_mpu->initialize();
delay(250);
if (!_mpu->testConnection()) {
ESP_LOGE(_TAG, "MPU6050 test connection failed!");
return false;
}
_mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
if (_mpu->dmpInitialize()) {
ESP_LOGE(_TAG, "Failed to initialize DMP!");
return false;
}
_mpu->setDMPEnabled(true);
attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING);
return true;
}
bool tick(bool &err) {
err = false;
if (!_isDMPDataReady) {
return false;
}
if (!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) {
ESP_LOGE(_TAG, "Failed to get DMP data!");
err = true;
return false;
}
Quaternion q;
VectorFloat gravity;
VectorInt16 accel;
VectorInt16 accelReal;
_mpu->dmpGetQuaternion(&q, _fifoBuffer);
_mpu->dmpGetGravity(&gravity, &q);
_mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
_mpu->dmpGetAccel(&accel, _fifoBuffer);
_mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
_isDMPDataReady = false;
_ax = accel.x / 8192.f;
_ay = accel.y / 8192.f;
_az = accel.z / 8192.f;
_calculateZInertial(gravity);
return true;
}
/* getters */
float accZInertial() {
return _AccZInertial;
}
float yaw() {
return degrees(_ypr[0]);
};
float pitch() {
return degrees(_ypr[1]);
};
float roll() {
return degrees(_ypr[2]);
};
private:
MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
float _ypr[3];
float _ax, _ay, _az;
float _AccZInertial = 0;
uint8_t _fifoBuffer[45];
//volatile bool _isDMPDataReady = false;
const char *_TAG = "MPU6050 module";
void _calculateZInertial(VectorFloat &gravity) {
const float anglePitch = _ypr[1];
const float angleRoll = _ypr[2];
_AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + cos(anglePitch) * cos(angleRoll) * _az;
_AccZInertial = (_AccZInertial - 1) * gravity.z * 100;
}
};

73
RF/BluetoothDispatcher.cpp Normal file
View File

@@ -0,0 +1,73 @@
#include "BluetoothDispatcher.hpp"
static DeviceConnectedCb deviceConnectedCallback = nullptr;
static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param);
BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char *device_name, HardwareSerial *serial) {
_device_name = device_name;
_controller = controller;
_serial = serial;
}
bool BluetoothDispatcher::initialize() {
_controller->enableSSP();
_controller->onConfirmRequest([this](uint16_t pin) {
_confirmRequestCallback(pin);
});
_controller->onAuthComplete([this](boolean success) {
_authCompleteCallback(success);
});
_controller->register_callback(deviceConnectedStaticCallback);
deviceConnectedCallback = [this](esp_bd_addr_t bt_addr) {
_deviceConnectedCallback(bt_addr);
};
_controller->setTimeout(2);
return _controller->begin(_device_name);
}
void BluetoothDispatcher::tick() {
while (_controller->available() and _buffer.length() <= _buffer_size) {
_buffer += (char)_controller->read();
}
if (_buffer.endsWith("\r")) {
_serial->println(_buffer.substring(0, _buffer.lastIndexOf("\r")));
_buffer.clear();
_controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!"));
}
if (_buffer.length() > _buffer_size) {
_buffer.clear();
}
}
BluetoothDispatcher::~BluetoothDispatcher() {
_controller->end();
delete _controller;
}
void BluetoothDispatcher::_confirmRequestCallback(uint16_t pin) {
_confirmRequestDone = true;
_serial->print("The PIN is: ");
_serial->println(pin);
_controller->confirmReply(true);
}
void BluetoothDispatcher::_authCompleteCallback(boolean success) {
if (success) {
_confirmRequestDone = true;
_serial->println("Pairing success!");
} else {
_serial->println("Pairing failed, rejected by user!");
}
}
void BluetoothDispatcher::_deviceConnectedCallback(esp_bd_addr_t bt_addr) {
_serial->print("New connection opened: ");
_serial->println(BTAddress(bt_addr).toString(true));
}
static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) {
if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS and deviceConnectedCallback) {
deviceConnectedCallback(param->srv_open.rem_bda);
}
}

42
RF/BluetoothDispatcher.hpp Normal file
View File

@@ -0,0 +1,42 @@
#include "Arduino.h"
#include "BluetoothSerial.h"
#include "HardwareSerial.h"
#include "Stream.h"
/* Check the ESP configuration */
#if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#if not defined(CONFIG_BT_SPP_ENABLED)
#error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif
#if not defined(CONFIG_BT_SSP_ENABLED)
#error Simple Secure Pairing for Bluetooth is not available or not enabled.
#endif
/* ************************* */
typedef std::function<void(esp_bd_addr_t bt_addr)> DeviceConnectedCb;
class BluetoothDispatcher {
public:
BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter", HardwareSerial *serial = &Serial);
bool initialize();
void tick();
~BluetoothDispatcher();
private:
void _confirmRequestCallback(uint16_t pin);
void _authCompleteCallback(boolean success);
void _deviceConnectedCallback(esp_bd_addr_t bt_addr);
const char *_device_name = nullptr;
bool _confirmRequestDone = false;
BluetoothSerial *_controller = nullptr;
HardwareSerial *_serial = nullptr;
static constexpr int _buffer_size = 256;
String _buffer;
};

92
arduino-firmware.ino
View File

@@ -1,92 +0,0 @@
#include "Barometer.h"
#include "GyverBME280.h"
#include "Wire.h"
#include "board_pins.h"
#include "I2Cdev.h"
#include "Kalman2DFilter.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "BatteryController.h"
#include "MPU.h"
TwoWire i2c = TwoWire(0);
Barometer barometer(new GyverBME280(i2c));
MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
BatteryController battery;
void setup() {
Serial.begin(115200);
i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000);
Serial.println(barometer.initialize());
Serial.println(mpu.initialize());
barometer.measureBaseAltitudeSync();
battery.initialize();
Serial.println("System initialized");
}
void loop() {
/*Serial.print("ax:");
Serial.print(ax);
Serial.print(",");
Serial.print("ay:");
Serial.print(ay);
Serial.print(",");
Serial.print("az:");
Serial.print(az);
Serial.print(",");*/
bool err;
barometer.tick();
if(mpu.tick(err) && !err) {
float kalmanAltitude, ZVelocityAltitude;
float barometerAltitude = barometer.rawFlightHeight();
kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
//Serial.print("Yaw:");
//Serial.print(mpu.yaw());
//Serial.print(",");
//Serial.print("Roll:");
//Serial.print(mpu.roll());
//Serial.print(",");
//Serial.print("Pitch:");
//Serial.print(mpu.pitch());
//Serial.print(",");
//Serial.print("Flight height:");
Serial.print(barometerAltitude);
Serial.print(",");
//Serial.print("Kalman altitude:");
Serial.print(kalmanAltitude);
Serial.print(",");
/*Serial.print(G);
Serial.print(",");
Serial.print(Pc);
Serial.print(",");
Serial.print(P);
Serial.print(",");
Serial.print(Xp);
Serial.print(",");*/
//Serial.print("Kalman2 altitude: ");
Serial.println(ZVelocityAltitude);
/*Serial.print("Vertical velocity:");
Serial.print(ZVelocityAltitude);
Serial.print(",");
Serial.print("Battery percent: ");
Serial.println(battery.percent());*/
}
//Serial.print(",");
/*
//Serial.print("AccZInertial:");
Serial.print(AccZInertial);
Serial.print(",");
//Serial.print("Altitude:");
Serial.print(AltitudeBarometer);
Serial.print(",");
//Serial.print("kAltitude:");
Serial.print(AltitudeKalman);
Serial.print(",");
//Serial.print("Battery voltage: ");
//Serial.print(battery.measureVoltage());
//Serial.print(",");
//Serial.print("kZvelocity:");
Serial.println(VelocityVerticalKalman);
}*/
}

56
main.cpp Normal file
View File

@@ -0,0 +1,56 @@
#include "Barometer.h"
#include "BatteryController.h"
#include "Filters/Kalman2DFilter.hpp"
#include "GyverBME280.h"
#include "I2Cdev.h"
#include "MPU.hpp"
#include "MPU6050_6Axis_MotionApps20.h"
#include "RF/BluetoothDispatcher.hpp"
#include "Wire.h"
#include "board_pins.h"
#include <Arduino.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;
BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
void setup() {
Serial.begin(115200);
Serial.print("Ininitialize I2C...");
Serial.println(
i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
Serial.print("Ininitialize BMP280...");
Serial.println(
barometer.initialize());
Serial.print("Ininitialize MPU6050...");
Serial.println(
mpu.initialize());
Serial.print("Ininitialize Bluetooth...");
Serial.println(
bluetoothDispatcher.initialize());
battery.initialize();
barometer.measureBaseAltitudeSync();
Serial.println("System initialized");
}
void loop() {
bool err;
barometer.tick();
if (mpu.tick(err) && !err) {
float kalmanAltitude, ZVelocityAltitude;
float barometerAltitude = barometer.rawFlightHeight();
kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
//Serial.print(barometerAltitude);
}
bluetoothDispatcher.tick();
delay(1);
}