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

4
RF/BluetoothDispatcher.cpp
View File

@@ -45,9 +45,9 @@ void BluetoothDispatcher::tick() {
while (_controller->available() and _buffer.length() <= _buffer_size) {
_buffer += (char)_controller->read();
}
if (_buffer.endsWith("\r")) {
if (_buffer.endsWith("\n\r")) {
char buffer[_buffer_size];
_buffer.substring(0, _buffer.lastIndexOf('\r'), buffer);
_buffer.substring(0, _buffer.lastIndexOf('}'), buffer);
ESP_LOGD(_tag, "Received new buffer %s", buffer);
if (_newPackageReceivedCb) {
_newPackageReceivedCb(buffer);

6
Sensors/MPU.hpp
View File

@@ -127,14 +127,14 @@ class MPU {
if (_isCalibration) {
return degrees(_ypr[1]);
} else {
return degrees(_ypr[1] - _yprOffset[1]);
return degrees(_ypr[1] - _prOffset[0]);
}
}
float roll() const {
if (_isCalibration) {
return degrees(_ypr[2]);
} else {
return degrees(_ypr[2] - _yprOffset[2]);
return degrees(_ypr[2] - _prOffset[1]);
}
}
@@ -204,7 +204,7 @@ class MPU {
float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
float _prOffset[2]; // yaw isn't used
float _prOffset[2]; // yaw isn't used
uint8_t _fifoBuffer[45];
Preferences _preferences;

3
main.cpp
View File

@@ -1,6 +1,7 @@
#include "App.hpp"
#include "BoardI2C.hpp"
//#include "MPU6050_6Axis_MotionApps20.h"
#define IGNORE_CHARGE true
BoardI2C i2c;
/*
@@ -18,7 +19,7 @@ void setup() {
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))));
new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
/*sensors = new Sensors(
new Barometer(new GyverBME280(i2c)), new MPU(new
MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),