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

PID controllers hierarchy revision

Browse Source
This commit is contained in:
Goga Coder
2024-03-08 23:10:55 +07:00
parent 271bd845bf
commit 4a85827150
16 changed files with 389 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.idea/.gitignore generated vendored Normal file
View File

@@ -0,0 +1,8 @@
# Default ignored files
/shelf/
/workspace.xml
# Editor-based HTTP Client requests
/httpRequests/
# Datasource local storage ignored files
/dataSources/
/dataSources.local.xml

5
.idea/codeStyles/codeStyleConfig.xml generated Normal file
View File

@@ -0,0 +1,5 @@
<component name="ProjectCodeStyleConfiguration">
<state>
<option name="USE_PER_PROJECT_SETTINGS" value="true" />
</state>
</component>

17
.idea/misc.xml generated Normal file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ExternalStorageConfigurationManager" enabled="true" />
<component name="PlatformIOSettings">
<option name="linkedExternalProjectsSettings">
<PlatformioProjectSettings>
<option name="externalProjectPath" value="$PROJECT_DIR$" />
<option name="modules">
<set>
<option value="$PROJECT_DIR$" />
</set>
</option>
</PlatformioProjectSettings>
</option>
</component>
<component name="PlatformIOWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
</project>

6
.idea/vcs.xml generated Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

3
src/.clang-format
View File

@@ -92,7 +92,6 @@ IncludeIsMainRegex: ''
IncludeIsMainSourceRegex: ''
IndentGotoLabels: false
IndentPPDirectives: None
IndentRequires: true
IndentWidth: 2
IndentWrappedFunctionNames: false
InsertTrailingCommas: None
@@ -148,7 +147,7 @@ StatementAttributeLikeMacros:
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
TabWidth: 2
#TabWidth: 2
UseCRLF: false
UseTab: Never
WhitespaceSensitiveMacros:

2
src/BoardI2C.hpp
View File

@@ -1,6 +1,7 @@
// 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
#define CONFIG_DISABLE_HAL_LOCKS 0
#include "Wire.h"
#include "board_pins.h"
#include "esp_log.h"
@@ -9,6 +10,7 @@ class BoardI2C : public TwoWire {
public:
BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
setTimeOut(50);
ESP_LOGI("I2CBus", "Bus initialized");
} else {
ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");

127
src/Logic/FlightController.cpp
View File

@@ -1,48 +1,59 @@
#include "FlightController.hpp"
FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
PIDController *pid3, bool unlimitedBattery) {
FlightController::FlightController(
Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
BrushedMotor *rotor1, BrushedMotor *rotor2,
PIDController *tailRotorController, bool unlimitedBattery) {
assert(sensors != nullptr);
assert(pid1 != nullptr);
assert(pid2 != nullptr);
assert(pid3 != nullptr);
assert(heightController != nullptr);
assert(yawController != nullptr);
assert(tailRotorController != nullptr);
assert(rotor1 != nullptr);
assert(rotor2 != nullptr);
_sensors = sensors;
_unlimitedBattery = unlimitedBattery;
_pid1 = pid1;
_pid2 = pid2;
_pid3 = pid3;
_heightController = heightController;
_yawController = yawController;
_tailRotorController = tailRotorController;
_rotor1 = rotor1;
_rotor2 = rotor2;
_status = DeviceStatus::Idle;
_updateBatteryCharge();
_checkBatteryCharge();
stopAllRotors();
ESP_LOGI(_tag, "Flight controller initialized");
}
FlightController::~FlightController() {
delete _sensors;
delete _pid1;
delete _pid2;
delete _pid3;
delete _heightController;
delete _yawController;
delete _tailRotorController;
delete _rotor1;
delete _rotor2;
}
void FlightController::tick() {
bool hasUpdates = _sensors->tick();
if (hasUpdates) {
_updateBatteryCharge();
_checkBatteryCharge();
}
switch (_status) {
case DeviceStatus::Idle: break;
case DeviceStatus::ChargeRequired: break;
case DeviceStatus::IsPreparingForTakeoff: break;
case DeviceStatus::IsImuCalibration: break;
case DeviceStatus::Idle:
break;
case DeviceStatus::ChargeRequired:
break;
case DeviceStatus::IsPreparingForTakeoff:
break;
case DeviceStatus::IsImuCalibration:
break;
case DeviceStatus::IsBoarding:
/* TODO: implement boarding */
break;
case DeviceStatus::IsFlying:
/* TODO: implement flying */
if (hasUpdates) {
_pid1->tick();
_pid2->tick();
_pid3->tick();
_tailRotorController->tick();
}
break;
}
@@ -57,7 +68,7 @@ ReportedDeviceState FlightController::currentDeviceState() const {
void FlightController::startTakeoff() {
/* Start preparation for takeoff */
_updateBatteryCharge();
_checkBatteryCharge();
if (_status != DeviceStatus::Idle)
return;
@@ -77,7 +88,7 @@ void FlightController::startBoarding() {
}
}
void FlightController::_updateBatteryCharge() {
void FlightController::_checkBatteryCharge() {
if (_unlimitedBattery)
return;
if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
@@ -92,16 +103,18 @@ void FlightController::_updateBatteryCharge() {
void FlightController::_takeoff() {
// TODO: implement takeoff
_status = DeviceStatus::IsFlying;
if (_onTakeoffStarted) {
_onTakeoffStarted();
}
_tailRotorController->enable();
_rotor1->enable();
_rotor2->enable();
}
void FlightController::_boarding() {
if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
// TODO: stop motors and setpoints
_status = DeviceStatus::Idle;
_rotor1->disable();
_rotor2->disable();
_tailRotorController->disable();
} else {
// TODO: implement boarding
}
@@ -134,63 +147,61 @@ void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
// TODO: implement stick input
}
void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
void FlightController::setRotor1Duty(uint32_t rotor1Duty) {
if (_status == DeviceStatus::Idle) {
_pid1->setRotorDuty(rotor1Duty);
_rotor1->setDuty(rotor1Duty);
}
}
void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
void FlightController::setRotor2Duty(uint32_t rotor2Duty) {
if (_status == DeviceStatus::Idle) {
_pid2->setRotorDuty(rotor2Duty);
_rotor2->setDuty(rotor2Duty);
}
}
void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
void FlightController::setRotor3Duty(uint32_t rotor3Duty) {
if (_status == DeviceStatus::Idle) {
_pid3->setRotorDuty(rotor3Duty);
_tailRotorController->setRotorDuty(rotor3Duty);
}
}
void FlightController::stopAllRotors() {
_pid1->setRotorDuty(0);
_pid2->setRotorDuty(0);
_pid3->setRotorDuty(0);
setRotor1Duty(0);
setRotor2Duty(0);
setRotor3Duty(0);
}
void FlightController::setPid1Params(float p, float i, float d) {
void FlightController::setHeightControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid1->Kp = p;
_pid1->Ki = i;
_pid1->Kd = d;
_pid1->save();
_heightController->Kp = p;
_heightController->Ki = i;
_heightController->Kd = d;
_heightController->save();
}
}
void FlightController::setPid2Params(float p, float i, float d) {
void FlightController::setYawControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid2->Kp = p;
_pid2->Ki = i;
_pid2->Kd = d;
_pid1->save();
_yawController->Kp = p;
_yawController->Ki = i;
_yawController->Kd = d;
_heightController->save();
}
}
void FlightController::setPid3Params(float p, float i, float d) {
void FlightController::setPitchControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid3->Kp = p;
_pid3->Ki = i;
_pid3->Kd = d;
_pid1->save();
_tailRotorController->Kp = p;
_tailRotorController->Ki = i;
_tailRotorController->Kd = d;
_heightController->save();
}
}
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);
PidSettings FlightController::pidSettings() const {
PidSettings settings;
settings.flightController = {.p = _heightController->Kp, .i = _heightController->Ki, .d = _heightController->Kd};
settings.yawController = {.p = _yawController->Kp, .i = _yawController->Ki, .d = _yawController->Kd};
settings.pitchController = {.p = _tailRotorController->Kp, .i = _tailRotorController->Ki, .d = _tailRotorController->Kd};
return settings;
}

66
src/Logic/FlightController.hpp
View File

@@ -1,7 +1,7 @@
// 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 "PIDController.hpp"
#include "Sensors/Sensors.hpp"
#include <memory>
@@ -27,51 +27,67 @@ struct FlightParams {
float pitch; // [degrees]
};
struct PidParam {
float p;
float i;
float d;
};
struct PidSettings {
PidParam pid1;
PidParam pid2;
PidParam pid3;
PidParams flightController;
PidParams yawController;
PidParams pitchController;
};
class FlightController {
public:
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
PIDController *pid3, bool unlimitedBattery = false);
FlightController(Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
BrushedMotor *_rotor1, BrushedMotor *_rotor2,
PIDController *tailRotor, bool unlimitedBattery);
~FlightController();
ReportedDeviceState currentDeviceState() const;
[[nodiscard]] ReportedDeviceState currentDeviceState() const;
void startTakeoff();
void startBoarding();
void startImuCalibration();
void tick();
[[deprecated]] void moveToFlightHeight(float newFlightHeight); //[cm]
[[deprecated]] void newFlightHeightStickPosition(int16_t newFlightHeightStickPostition);
[[deprecated]] void newYawStickPosition(int16_t newYawStickPostition);
[[deprecated]] void newPitchStickPosition(int16_t newPitchStickPostition);
void newRotor1Duty(uint32_t rotor1Duty);
void newRotor2Duty(uint32_t rotor2Duty);
void newRotor3Duty(uint32_t rotor3Duty);
void setRotor1Duty(uint32_t rotor1Duty);
void setRotor2Duty(uint32_t rotor2Duty);
void setRotor3Duty(uint32_t rotor3Duty);
void stopAllRotors();
void setPid1Params(float p, float i, float d);
void setPid2Params(float p, float i, float d);
void setPid3Params(float p, float i, float d);
std::unique_ptr<PidSettings> pidSettings() const;
void setHeightControllerParams(float p, float i, float d);
void setYawControllerParams(float p, float i, float d);
void setPitchControllerParams(float p, float i, float d);
[[nodiscard]] PidSettings pidSettings() const;
private:
void _updateBatteryCharge();
void _checkBatteryCharge();
[[deprecated]] void _takeoff();
[[deprecated]] void _boarding();
Sensors *_sensors = nullptr;
PIDController *_pid1 = nullptr;
PIDController *_pid2 = nullptr;
PIDController *_pid3 = nullptr;
SavedPidRegulator *_heightController = nullptr;
SavedPidRegulator *_yawController = nullptr;
PIDController *_tailRotorController = nullptr;
BrushedMotor *_rotor1 = nullptr;
BrushedMotor *_rotor2 = nullptr;
DeviceStatus _status;
OnMeasuringFinishedCb _onTakeoffStarted = nullptr;
static constexpr const char *_tag = "FlightController";
static constexpr uint8_t _batteryCriticalCharge = 15; // [%]

77
src/Logic/FlightDispatcher.cpp
View File

@@ -22,8 +22,7 @@ FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
ESP_LOGI(_tag, "Bluetooth initialized successfully.");
} else {
ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!");
while (loop_on_fail)
;
while (loop_on_fail);
}
_telemetryTimer = millis();
}
@@ -66,44 +65,59 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
}
switch (pkg.keyHash(keyIndex)) {
case SH("status"): _changeStatus((DeviceStatus)value.toInt32()); break;
case SH("status"):
_changeStatus((DeviceStatus) value.toInt32());
break;
case SH("flightHeight"):
_flightController->moveToFlightHeight(value.toFloat());
break;
case SH("hS"):
_flightController->newFlightHeightStickPosition(value.toInt16());
break;
case SH("yS"): _flightController->newYawStickPosition(value.toInt16()); break;
case SH("yS"):
_flightController->newYawStickPosition(value.toInt16());
break;
case SH("pS"):
_flightController->newPitchStickPosition(value.toInt16());
break;
case SH("r1"): _flightController->newRotor1Duty(value.toInt32()); break;
case SH("r2"): _flightController->newRotor2Duty(value.toInt32()); break;
case SH("r3"): _flightController->newRotor3Duty(value.toInt32()); break;
case SH("stop"): _flightController->stopAllRotors(); break;
case SH("r1"):
_flightController->setRotor1Duty(value.toInt32());
break;
case SH("r2"):
_flightController->setRotor2Duty(value.toInt32());
break;
case SH("r3"):
_flightController->setRotor3Duty(value.toInt32());
break;
case SH("stop"):
_flightController->stopAllRotors();
break;
case SH("p1"):
if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
and pair.includes(SH("i")) and pair.includes(SH("d"))) {
_flightController->setPid1Params(
_flightController->setHeightControllerParams(
pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
}
break;
case SH("p2"):
if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
and pair.includes(SH("i")) and pair.includes(SH("d"))) {
_flightController->setPid2Params(
_flightController->setYawControllerParams(
pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
}
break;
case SH("p3"):
if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
and pair.includes(SH("i")) and pair.includes(SH("d"))) {
_flightController->setPid3Params(
_flightController->setPitchControllerParams(
pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
}
break;
case SH("pidSettingOpened"): _pidSettingsOpened(); break;
default: break;
case SH("pidSettingOpened"):
_pidSettingsOpened();
break;
default:
break;
}
}
}
@@ -111,13 +125,20 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) {
switch (newStatus) {
case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break;
case DeviceStatus::IsFlying:
_changeStatus(DeviceStatus::IsPreparingForTakeoff);
case DeviceStatus::IsImuCalibration:
_flightController->startImuCalibration();
break;
case DeviceStatus::IsFlying:
_flightController->startTakeoff();
break;
case DeviceStatus::IsPreparingForTakeoff:
_flightController->startTakeoff();
break;
case DeviceStatus::IsBoarding:
_flightController->startBoarding();
break;
default:
break;
case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break;
case DeviceStatus::IsBoarding: _flightController->startBoarding(); break;
default: break;
}
}
@@ -127,21 +148,21 @@ void FlightDispatcher::_pidSettingsOpened() {
pkg.beginObj();
pkg.beginObj("p1");
pkg["p"] = settings->pid1.p;
pkg["i"] = settings->pid1.i;
pkg["d"] = settings->pid1.d;
pkg["p"] = settings.flightController.p;
pkg["i"] = settings.flightController.i;
pkg["d"] = settings.flightController.d;
pkg.endObj();
pkg.beginObj("p2");
pkg["p"] = settings->pid2.p;
pkg["i"] = settings->pid2.i;
pkg["d"] = settings->pid2.d;
pkg["p"] = settings.yawController.p;
pkg["i"] = settings.yawController.i;
pkg["d"] = settings.yawController.d;
pkg.endObj();
pkg.beginObj("p3");
pkg["p"] = settings->pid3.p;
pkg["i"] = settings->pid3.i;
pkg["d"] = settings->pid3.d;
pkg["p"] = settings.pitchController.p;
pkg["i"] = settings.pitchController.i;
pkg["d"] = settings.pitchController.d;
pkg.endObj();
pkg.endObj();

59
src/Logic/PID.hpp
View File

@@ -1,59 +0,0 @@
// 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"
#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;
this->_dt = dt;
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 setRotorDuty(uint32_t duty) {
/* Used only for test purposes */
_rotor->setDuty(duty);
}
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);
}
}
};

46
src/Logic/PIDController.hpp Normal file
View File

@@ -0,0 +1,46 @@
// 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 "Motor/BrushedMotor.hpp"
#include "SavedPidRegulator.hpp"
#include "esp_log.h"
class PIDController : public SavedPidRegulator {
public:
PIDController(float p, float i, float d, BrushedMotor *rotor,
const char *name = "PID", uint16_t dt = 10)
: SavedPidRegulator(p, i, d, name, dt) {
assert(rotor != nullptr);
_rotor = rotor;
this->_dt = dt;
setLimits(0, _rotor->maxDuty());
_pid_timer = millis();
ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
}
void enable() {
_rotor->enable();
}
void disable() {
_rotor->disable();
}
void tick() {
if (millis() - _pid_timer >= _dt) {
_rotor->setDuty((uint32_t) getResultTimer());
_pid_timer = millis();
}
}
void setRotorDuty(uint32_t duty) {
/* Used only for test purposes */
_rotor->setDuty(duty);
}
private:
uint16_t _dt;
BrushedMotor *_rotor = nullptr;
uint32_t _pid_timer = 0;
};

71
src/Logic/SavedPidRegulator.hpp Normal file
View File

@@ -0,0 +1,71 @@
#include "GyverPID.h"
#include "Preferences.h"
#include "esp_log.h"
#include <memory>
struct PidParams {
float p;
float i;
float d;
};
class SavedPidRegulator : public GyverPID {
public:
SavedPidRegulator(float p, float i, float d, const char *name = "saved_pid", uint16_t dt = 10)
: GyverPID(p, i, d, (int16_t) dt) {
_name = name;
_preferences.begin(name);
_pDefault = p;
_iDefault = i;
_dDefault = d;
_loadPreferences();
ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
}
void setParams(PidParams params) {
this->Kp = params.p;
this->Ki = params.i;
this->Kd = params.d;
}
std::shared_ptr<PidParams> getParams() {
std::shared_ptr params = std::make_unique<PidParams>();
params->p = Kp;
params->i = Ki;
params->d = Kd;
return params;
}
void resetToDefault() {
this->Kp = _pDefault;
this->Ki = _iDefault;
this->Kd = _dDefault;
save();
}
void save() {
_preferences.putFloat("p", Kp);
_preferences.putFloat("i", Ki);
_preferences.putFloat("d", Kd);
ESP_LOGI(_tag, "PID %s saved with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
}
protected:
static constexpr const char *_tag = "PIDController";
private:
float _pDefault = 0.f;
float _iDefault = 0.f;
float _dDefault = 0.f;
String _name = "PID";
Preferences _preferences;
void _loadPreferences() {
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);
}
}
};

8
src/Motor/BrushedMotor.hpp
View File

@@ -11,13 +11,21 @@ class BrushedMotor {
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:

12
src/RF/BluetoothDispatcher.cpp
View File

@@ -4,6 +4,7 @@
#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) {
@@ -26,12 +27,11 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
deviceConnectedCallback = [this](BTAddress device) {
_onDeviceConnected(device);
};
_controller->setTimeout(2);
_controller->setTimeout(50);
bool success = _controller->begin(_device_name, false);
if (!success) {
ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
while (loop_on_fail)
;
while (loop_on_fail);
return false;
} else {
ESP_LOGI(_tag, "Bluetooth host initialized");
@@ -43,12 +43,12 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
_newPackageReceivedCb = newPackageReceivedCb;
}
void BluetoothDispatcher::tick(const char *message_delimeter) {
void BluetoothDispatcher::tick(const char message_delimeter) {
/* Call the callback, if new package received */
while (_controller->available()) { _buffer += (char) _controller->read(); }
if (_buffer.endsWith(message_delimeter)) {
if (_buffer.lastIndexOf(message_delimeter) != -1) {
char buffer[_buffer_size];
auto messageEnd = _buffer.lastIndexOf('}');
auto messageEnd = _buffer.indexOf(message_delimeter) - 1;
if (messageEnd == -1) {
_buffer.clear();
} else {

2
src/RF/BluetoothDispatcher.hpp
View File

@@ -29,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(const char *message_delimeter = "\n");
void tick(const char message_delimeter = '\n');
void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
void sendPackage(const char *package, size_t size);

12
src/main.cpp
View File

@@ -14,11 +14,15 @@ void setup() {
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 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), true)));
new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightController"), // height
new SavedPidRegulator(2.f, 12.f, 0.f, "YawController"), // yaw
new BrushedMotor(1, 2, 3, 4, 1),
new BrushedMotor(1, 2, 3, 4, 1),
new PIDController(0.6f, 3.5f, 0.03f, new BrushedMotor(1, 2, 3, 4, 3), "RollController"), // pitch
true)));
}
void loop() {