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

Better routing

Browse Source
This commit is contained in:
Goga Coder
2024-01-18 19:30:25 +07:00
parent 5dd404d44b
commit 2aa9648dd4
8 changed files with 247 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
Logic/FlightController.cpp
View File

@@ -1,13 +1,16 @@
#include "FlightController.hpp"
FlightController::FlightController(Sensors *sensors, PIDController *pid1,
PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
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;
_pid1 = pid1;
_pid2 = pid2;
_pid3 = pid3;
_status = DeviceStatus::Idle;
_updateBatteryCharge();
@@ -30,11 +33,17 @@ void FlightController::tick() {
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();
}
break;
}
}
@@ -46,10 +55,10 @@ ReportedDeviceState FlightController::currentDeviceState() const {
.mpuState = _sensors->mpuData() };
}
void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) {
void FlightController::startTakeoff() {
/* Start preparation for takeoff */
_updateBatteryCharge();
if (_status == DeviceStatus::ChargeRequired)
if (_status != DeviceStatus::Idle)
return;
_status = DeviceStatus::IsPreparingForTakeoff;
@@ -69,11 +78,11 @@ void FlightController::startBoarding() {
}
void FlightController::_updateBatteryCharge() {
if(_unlimitedBattery) return;
if (_unlimitedBattery)
return;
if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) {
// TODO: board the device
_status = DeviceStatus::IsBoarding;
if (_status == DeviceStatus::IsFlying) {
startBoarding();
} else {
_status = DeviceStatus::ChargeRequired;
}
@@ -83,7 +92,7 @@ void FlightController::_updateBatteryCharge() {
void FlightController::_takeoff() {
// TODO: implement takeoff
_status = DeviceStatus::IsFlying;
if(_onTakeoffStarted) {
if (_onTakeoffStarted) {
_onTakeoffStarted();
}
}
@@ -92,13 +101,91 @@ void FlightController::_boarding() {
if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
// TODO: stop motors and setpoints
_status = DeviceStatus::Idle;
} else {
// TODO: implement boarding
}
}
void FlightController::startImuCalibration() {
if (_status != DeviceStatus::Idle)
return;
_sensors->onMpuCalibrationFinished([this]() {
this->_status = DeviceStatus::Idle;
});
}
void FlightController::moveToFlightHeight(float newFlightHeight) {
// TODO: implement setting flight height
}
void FlightController::newFlightHeightStickPosition(int16_t newFlightHeightStickPostition) {
// TODO: implement stick input
}
void FlightController::newYawStickPosition(int16_t newYawStickPostition) {
// TODO: implement stick input
}
void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
// TODO: implement stick input
}
void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
if (_status == DeviceStatus::Idle) {
_pid1->setRotorDuty(rotor1Duty);
}
}
void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
if (_status == DeviceStatus::Idle) {
_pid2->setRotorDuty(rotor2Duty);
}
}
void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
if (_status == DeviceStatus::Idle) {
_pid3->setRotorDuty(rotor3Duty);
}
}
void FlightController::stopAllRotors() {
_pid1->setRotorDuty(0);
_pid2->setRotorDuty(0);
_pid3->setRotorDuty(0);
}
void FlightController::setPid1Params(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid1->Kp = p;
_pid1->Ki = i;
_pid1->Kd = d;
_pid1->save();
}
}
void FlightController::setPid2Params(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid2->Kp = p;
_pid2->Ki = i;
_pid2->Kd = d;
_pid1->save();
}
}
void FlightController::setPid3Params(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_pid3->Kp = p;
_pid3->Ki = i;
_pid3->Kd = d;
_pid1->save();
}
}
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 };
PidSettings settings[] = { pid1, pid2, pid3 };
return settings;
}

35
Logic/FlightController.hpp
View File

@@ -18,24 +18,43 @@ struct ReportedDeviceState {
};
struct FlightParams {
float height; // [cm]
float yaw; // [degrees]
float pitch; // [degrees]
float height; // [cm]
float yaw; // [degrees]
float pitch; // [degrees]
};
struct PidSettings {
float p;
float i;
float d;
};
class FlightController {
public:
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, PIDController *pid3, bool unlimitedBattery = false);
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
PIDController *pid3, bool unlimitedBattery = false);
~FlightController();
ReportedDeviceState currentDeviceState() const;
void startTakeoff(OnMeasuringFinishedCb onTakeoffStarted);
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 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);
PidSettings *pidSettings() const;
private:
void _updateBatteryCharge();
void _takeoff();
void _boarding();
[[deprecated]] void _takeoff();
[[deprecated]] void _boarding();
Sensors *_sensors = nullptr;
PIDController *_pid1 = nullptr;
@@ -50,5 +69,5 @@ class FlightController {
static constexpr float _defaultHorizontAngle = 90.f; // [degrees]
static constexpr float _defaultStopMotorHeight = 5; // [cm]
bool _unlimitedBattery = false; // for debug purposes
bool _unlimitedBattery = false; // for debug purposes
};

101
Logic/FlightDispatcher.cpp
View File

@@ -54,10 +54,105 @@ void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
}
void FlightDispatcher::_onNewMessageReceived(char *package) {
ESP_LOGI(_tag, "Received new package: %s", package);
using sutil::SH;
ESP_LOGD(_tag, "Received new package: %s", package);
gson::Doc pkg;
pkg.parse(package, _jsonMaxDepth);
// TODO: process package
if (!pkg.parse(package, _jsonMaxDepth)) {
ESP_LOGE(_tag, "Parcing error occured with new package (error %s, place %d): %s",
pkg.readError(), pkg.errorIndex(), package);
} else {
pkg.hashKeys();
for (int keyIndex = 0; keyIndex < pkg.length(); ++keyIndex) {
auto pair = pkg[pkg.keyHash(keyIndex)];
auto value = pair.value();
ESP_LOGD(_tag, "Key: %zu", pkg.keyHash(keyIndex));
if (!pair.valid()) {
ESP_LOGW(_tag, "Pair isn't valid and was skipped");
continue;
}
switch (pkg.keyHash(keyIndex)) {
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("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("p1"):
if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
and pair.includes(SH("i")) and pair.includes(SH("d"))) {
_flightController->setPid1Params(
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(
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(
pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
}
break;
case SH("pidSettingOpened"): _pidSettingsOpened(); break;
default: break;
}
}
}
}
void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) {
switch (newStatus) {
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;
}
}
void FlightDispatcher::_pidSettingsOpened() {
auto settings = _flightController->pidSettings();
gson::string pkg;
pkg.beginObj();
pkg.beginObj("p1");
pkg["p"] = settings[0].p;
pkg["i"] = settings[0].i;
pkg["d"] = settings[0].d;
pkg.endObj();
pkg.beginObj("p2");
pkg["p"] = settings[1].p;
pkg["i"] = settings[1].i;
pkg["d"] = settings[1].d;
pkg.endObj();
pkg.beginObj("p3");
pkg["p"] = settings[2].p;
pkg["i"] = settings[2].i;
pkg["d"] = settings[2].d;
pkg.endObj();
pkg.endObj();
pkg.end();
pkg += "\r\n";
_bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
}
void FlightDispatcher::_sendTelemetry() {

17
Logic/FlightDispatcher.hpp
View File

@@ -1,19 +1,28 @@
#include "FlightController.hpp"
#include "GSON.h"
#include "RF/BluetoothDispatcher.hpp"
#include <map>
class FlightDispatcher {
/* Deserialize state and update it in FlightController. */
/* 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);
/* Telemetry flow */
void _sendTelemetry();
/* Events handlers */
void _onNewDeviceConnected(BTAddress device);
void _onNewMessageReceived(char *package);
/* Requests handlers */
void _changeStatus(const DeviceStatus &newStatus);
void _pidSettingsOpened();
/* Compile time settings */
static constexpr const char *_tag = "FlightDispatcher";
static constexpr const int _telemetryTimeIntervalMS = 200;
static constexpr const uint8_t _jsonMaxDepth = 5;
@@ -21,4 +30,6 @@ class FlightDispatcher {
uint32_t _telemetryTimer = millis();
BluetoothDispatcher *_bluetoothDispatcher;
FlightController *_flightController;
std::map<size_t, std::function<void(gson::Entry)>> _routes;
};

5
Logic/PID.hpp
View File

@@ -25,6 +25,11 @@ class PIDController : public GyverPID {
}
}
void setRotorDuty(uint32_t duty) {
/* Used only for test purposes */
_rotor->setDuty(duty);
}
void save() {
_preferences.putFloat("p", Kp);
_preferences.putFloat("i", Ki);