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

All modules and core of the app was created

Browse Source
This commit is contained in:
Goga Coder
2024-01-11 22:47:53 +07:00
parent 3c2889b8f7
commit 5dd404d44b
18 changed files with 647 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.clang-format
View File

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

23
App.hpp Normal file
View File

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

3
BoardI2C.h → BoardI2C.hpp
View File

@@ -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!");

20
Filters/Kalman2DFilter.hpp
View File

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

104
Logic/FlightController.cpp Normal file
View File

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

54
Logic/FlightController.hpp Normal file
View File

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

80
Logic/FlightDispatcher.cpp Normal file
View File

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

24
Logic/FlightDispatcher.hpp Normal file
View File

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

50
Logic/PID.hpp Normal file
View File

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

22
Motor/BrushedMotor.cpp Normal file
View File

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

29
Motor/BrushedMotor.hpp Normal file
View File

@@ -0,0 +1,29 @@
#include <stdint.h>
// 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;
};

23
RF/BluetoothDispatcher.cpp
View File

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

12
RF/BluetoothDispatcher.hpp
View File

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

22
Sensors/Barometer.hpp
View File

@@ -1,5 +1,7 @@
#include "GyverBME280.h"
typedef std::function<void()> 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]
};

156
Sensors/MPU.hpp
View File

@@ -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<void()> 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() {
}
float pitch() const {
if (_isCalibration) {
return degrees(_ypr[1]);
};
float roll() {
} else {
return degrees(_ypr[1] - _yprOffset[1]);
}
}
float roll() const {
if (_isCalibration) {
return degrees(_ypr[2]);
};
} else {
return degrees(_ypr[2] - _yprOffset[2]);
}
}
float ax() {
float ax() const {
if (_isCalibration) {
return _ax;
} else {
return _ax - _axOffset;
}
float ay() {
}
float ay() const {
if (_isCalibration) {
return _ay;
} else {
return _ay - _ayOffset;
}
float az() {
}
float az() const {
if (_isCalibration) {
return _az;
} else {
return _az - _azOffset;
}
}
float gravityZ() {
float gravityZ() const {
return _gravity;
}
float gx() {
float gx() const {
if (_isCalibration) {
return _gx;
} else {
return _gx - _gxOffset;
}
float gy() {
}
float gy() const {
if (_isCalibration) {
return _gy;
} else {
return _gy - _gyOffset;
}
float gz() {
}
float gz() const {
if (_isCalibration) {
return _gz;
} else {
return _gz - _gzOffset;
}
}
void startCalibration() {
_isCalibration = true;
}
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]);
}
};

41
Sensors/Sensors.cpp
View File

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

19
Sensors/Sensors.hpp
View File

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

70
main.cpp
View File

@@ -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 <Arduino.h>
#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);
}
app->tick();
/*
sensors->tick();
bluetoothDispatcher.tick();
delay(1);
delay(1);*/
}