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

Project structure redefinition

Browse Source
This commit is contained in:
Goga Coder
2024-03-01 22:58:59 +07:00
parent 9d0f5d06ea
commit 69c736b1eb
18 changed files with 126 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
platformio.ini
View File

@@ -24,7 +24,7 @@ lib_deps_external =
https://github.com/tomstewart89/BasicLinearAlgebra.git @ 4.3
https://github.com/GyverLibs/mString.git @ 1.7
https://github.com/GyverLibs/GyverPID @ 3.3
https://github.com/pololu/vl53l0x-arduino @ 1.3.1
[env:esp32dev]
platform = espressif32

3
src/App.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "Logic/FlightDispatcher.hpp"
#include "esp_log.h"

3
src/BoardI2C.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "Wire.h"
#include "board_pins.h"
#include "esp_log.h"

6
src/Filters/Kalman2DFilter.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include <BasicLinearAlgebra.h>
class Kalman2DFilter {
@@ -14,7 +17,8 @@ class Kalman2DFilter {
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;

20
src/Logic/FlightController.cpp
View File

@@ -1,6 +1,3 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "FlightController.hpp"
FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -113,6 +110,9 @@ void FlightController::_boarding() {
void FlightController::startImuCalibration() {
if (_status != DeviceStatus::Idle)
return;
ESP_LOGI(_tag, "Started IMU calibration");
_status = DeviceStatus::IsImuCalibration;
_sensors->startMpuCalibration();
_sensors->onMpuCalibrationFinished([this]() {
this->_status = DeviceStatus::Idle;
});
@@ -185,10 +185,12 @@ void FlightController::setPid3Params(float p, float i, float d) {
}
}
PidSettings *FlightController::pidSettings() const {
PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
static PidSettings settings[] = { pid1, pid2, pid3 };
return settings;
std::unique_ptr<PidSettings> FlightController::pidSettings() const {
std::unique_ptr<PidSettings> settings = std::make_unique<PidSettings>();
settings->pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
settings->pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
settings->pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
ESP_LOGI(_tag, "PID №1 settings: (%f, %f, %f)", settings->pid1.p, settings->pid1.i,
settings->pid1.d);
return std::move(settings);
}

15
src/Logic/FlightController.hpp
View File

@@ -1,6 +1,9 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "PID.hpp"
#include <memory>
#include "Sensors/Sensors.hpp"
#include <memory>
enum DeviceStatus {
Idle = 0,
@@ -24,12 +27,18 @@ struct FlightParams {
float pitch; // [degrees]
};
struct PidSettings {
struct PidParam {
float p;
float i;
float d;
};
struct PidSettings {
PidParam pid1;
PidParam pid2;
PidParam pid3;
};
class FlightController {
public:
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -51,7 +60,7 @@ class FlightController {
void setPid1Params(float p, float i, float d);
void setPid2Params(float p, float i, float d);
void setPid3Params(float p, float i, float d);
PidSettings *pidSettings() const;
std::unique_ptr<PidSettings> pidSettings() const;
private:
void _updateBatteryCharge();
[[deprecated]] void _takeoff();

40
src/Logic/FlightDispatcher.cpp
View File

@@ -43,22 +43,12 @@ void FlightDispatcher::tick() {
}
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));
_sendTelemetry();
}
void FlightDispatcher::_onNewMessageReceived(char *package) {
using sutil::SH;
ESP_LOGD(_tag, "Received new package: %s", package);
ESP_LOGI(_tag, "Received new package: %s", package);
gson::Doc pkg;
if (!pkg.parse(package, _jsonMaxDepth)) {
ESP_LOGE(_tag, "Parcing error occured with new package (error %s, place %d): %s",
@@ -137,27 +127,28 @@ void FlightDispatcher::_pidSettingsOpened() {
pkg.beginObj();
pkg.beginObj("p1");
pkg["p"] = settings[0].p;
pkg["i"] = settings[0].i;
pkg["d"] = settings[0].d;
pkg["p"] = settings->pid1.p;
pkg["i"] = settings->pid1.i;
pkg["d"] = settings->pid1.d;
pkg.endObj();
pkg.beginObj("p2");
pkg["p"] = settings[1].p;
pkg["i"] = settings[1].i;
pkg["d"] = settings[1].d;
pkg["p"] = settings->pid2.p;
pkg["i"] = settings->pid2.i;
pkg["d"] = settings->pid2.d;
pkg.endObj();
pkg.beginObj("p3");
pkg["p"] = settings[2].p;
pkg["i"] = settings[2].i;
pkg["d"] = settings[2].d;
pkg["p"] = settings->pid3.p;
pkg["i"] = settings->pid3.i;
pkg["d"] = settings->pid3.d;
pkg.endObj();
pkg.endObj();
pkg.end();
pkg += "\r\n";
pkg.s = String(MessageType::PidSettings) + ";" + pkg.s + _message_delimeter;
_bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
ESP_LOGI(_tag, "PID settings sended %s", pkg.s.c_str());
}
void FlightDispatcher::_sendTelemetry() {
@@ -165,7 +156,7 @@ void FlightDispatcher::_sendTelemetry() {
auto state = _flightController->currentDeviceState();
gson::string package;
package.beginObj();
package["batteryCharge"] = state.batteryCharge;
package["charge"] = state.batteryCharge;
package["flightHeight"] = state.flightHeight;
package["status"] = state.status;
package["y"] = state.mpuState.yaw;
@@ -174,7 +165,6 @@ void FlightDispatcher::_sendTelemetry() {
package["zIn"] = state.mpuState.zInertial;
package.endObj();
package.end();
package += "\r\n";
package.s = String(MessageType::UpdatePackage) + ";" + package.s + _message_delimeter;
_bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
}

9
src/Logic/FlightDispatcher.hpp
View File

@@ -1,8 +1,14 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "FlightController.hpp"
#include "GSON.h"
#include "RF/BluetoothDispatcher.hpp"
#include <map>
// Message type annotation for mobile app
enum MessageType { UpdatePackage = 0, PidSettings };
class FlightDispatcher {
/* Deserialize state and update it in FlightController. */
public:
@@ -26,10 +32,9 @@ class FlightDispatcher {
static constexpr const char *_tag = "FlightDispatcher";
static constexpr const int _telemetryTimeIntervalMS = 200;
static constexpr const uint8_t _jsonMaxDepth = 5;
static constexpr const char *_message_delimeter = "\n";
uint32_t _telemetryTimer = millis();
BluetoothDispatcher *_bluetoothDispatcher;
FlightController *_flightController;
std::map<size_t, std::function<void(gson::Entry)>> _routes;
};

3
src/Logic/PID.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "GyverPID.h"
#include "Motor/BrushedMotor.hpp"
#include "Preferences.h"

3
src/Motor/BrushedMotor.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include <stdint.h>
// TODO: implement class

20
src/RF/BluetoothDispatcher.cpp
View File

@@ -43,21 +43,24 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
_newPackageReceivedCb = newPackageReceivedCb;
}
void BluetoothDispatcher::tick() {
void BluetoothDispatcher::tick(const char *message_delimeter) {
/* Call the callback, if new package received */
while (_controller->available() and _buffer.length() <= _buffer_size) {
_buffer += (char)_controller->read();
}
if (_buffer.endsWith("\n\r")) {
while (_controller->available()) { _buffer += (char)_controller->read(); }
if (_buffer.endsWith(message_delimeter)) {
char buffer[_buffer_size];
_buffer.substring(0, _buffer.lastIndexOf('}'), buffer);
auto messageEnd = _buffer.lastIndexOf('}');
if (messageEnd == -1) {
_buffer.clear();
} else {
_buffer.substring(0, messageEnd, buffer);
}
ESP_LOGD(_tag, "Received new buffer %s", buffer);
if (_newPackageReceivedCb) {
_newPackageReceivedCb(buffer);
}
_buffer.clear();
}
if (_buffer.length() > _available_buffer_size) {
if (_buffer.length() > _real_buffer_size) {
_buffer.clear();
}
}
@@ -80,8 +83,9 @@ void BluetoothDispatcher::_onAuthComplete(boolean success) const {
}
}
void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const {
void BluetoothDispatcher::_onDeviceConnected(BTAddress device) {
ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str());
_buffer.clear();
if (_deviceConnectedCallback) {
_deviceConnectedCallback(device);
}

9
src/RF/BluetoothDispatcher.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "BluetoothSerial.h"
#include "HardwareSerial.h"
#include "esp_log.h"
@@ -26,7 +29,7 @@ class BluetoothDispatcher {
public:
BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2);
void tick();
void tick(const char *message_delimeter = "\n");
void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
void sendPackage(const char *package, size_t size);
@@ -35,7 +38,7 @@ class BluetoothDispatcher {
private:
void _onConfirmRequest(uint16_t pin);
void _onAuthComplete(boolean success) const;
void _onDeviceConnected(BTAddress device) const;
void _onDeviceConnected(BTAddress device);
const char *_device_name = nullptr;
BluetoothSerial *_controller = nullptr;
@@ -44,6 +47,6 @@ class BluetoothDispatcher {
constexpr static const char *_tag = "BluetoothDispatcher";
static constexpr uint16_t _buffer_size = 522;
static constexpr uint16_t _available_buffer_size = 512;
static constexpr uint16_t _real_buffer_size = 512;
mString<_buffer_size> _buffer;
};

3
src/Sensors/Barometer.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "GyverBME280.h"
typedef std::function<void()> OnMeasuringFinishedCb;

32
src/Sensors/BatteryController.hpp
View File

@@ -1,19 +1,39 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "board_pins.h"
class BatteryController {
public:
BatteryController() {}
BatteryController(const uint16_t batteryPin, const uint16_t battery_data_switch_pin) {
_battery_pin = batteryPin;
_battery_data_switch_pin = battery_data_switch_pin;
}
void initialize() {
pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
pinMode(_battery_data_switch_pin, OUTPUT);
digitalWrite(_battery_data_switch_pin, HIGH);
analogReadResolution(12);
analogSetWidth(12);
adcAttachPin(_battery_pin);
}
float measureVoltage() {
return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095;
return analogRead(BATTERY_DATA_PIN) * 3.3f / 4096.f;
}
int percent(int minVoltage = 7200, int maxVoltage = 8400) {
return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100);
int percent(const float minVoltage = 7.2f, const float maxVoltage = 8.4f,
const float r1 = 3.3f, const float r2 = 2.f, const float k = 2.87f) {
auto batteryVoltage = measureVoltage() * ((r1 + r2) / k);
return round(_map(batteryVoltage, minVoltage, maxVoltage, 5.f, 100.f));
}
private:
uint16_t _battery_pin;
uint16_t _battery_data_switch_pin;
static constexpr const char *_tag = "BatteryController";
float _map(float x, float in_min, float in_max, float out_min, float out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
};

6
src/Sensors/MPU.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "MPU6050_6Axis_MotionApps20.h"
#include "Preferences.h"
#include "board_pins.h"
@@ -98,9 +101,11 @@ class MPU {
_isCalibration = false;
_updateOffsets();
_calibrationsIterCounter = 0;
if (_calibrationFinishedCb) {
_calibrationFinishedCb();
}
ESP_LOGI(_TAG, "Calibration finished!");
} else {
_axOffset += _ax;
_ayOffset += _ay;
@@ -110,6 +115,7 @@ class MPU {
_gzOffset += _gz;
_prOffset[0] += _ypr[1];
_prOffset[1] += _ypr[2];
_calibrationsIterCounter++;
}
}

3
src/Sensors/Sensors.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "Barometer.hpp"
#include "BatteryController.hpp"
#include "Filters/Kalman2DFilter.hpp"

3
src/board_pins.h
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "driver/adc.h"
#define I2C_SDA_PIN 21

9
src/main.cpp
View File

@@ -4,8 +4,6 @@
#include "App.hpp"
#include "BoardI2C.hpp"
#define IGNORE_CHARGE true
BoardI2C i2c;
static Application *app = nullptr;
@@ -15,11 +13,12 @@ void setup() {
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 Sensors(new Barometer(new GyverBME280(i2c)),
new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),
new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN)),
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 1), 1),
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 2), 2),
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), true)));
}
void loop() {