diff --git a/Logic/FlightController.cpp b/Logic/FlightController.cpp index 40e5276..59eb649 100644 --- a/Logic/FlightController.cpp +++ b/Logic/FlightController.cpp @@ -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; } \ No newline at end of file diff --git a/Logic/FlightController.hpp b/Logic/FlightController.hpp index b337325..812ed49 100644 --- a/Logic/FlightController.hpp +++ b/Logic/FlightController.hpp @@ -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 }; \ No newline at end of file diff --git a/Logic/FlightDispatcher.cpp b/Logic/FlightDispatcher.cpp index dcf2a81..afa8bf2 100644 --- a/Logic/FlightDispatcher.cpp +++ b/Logic/FlightDispatcher.cpp @@ -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() { diff --git a/Logic/FlightDispatcher.hpp b/Logic/FlightDispatcher.hpp index c6a833f..af6b1d6 100644 --- a/Logic/FlightDispatcher.hpp +++ b/Logic/FlightDispatcher.hpp @@ -1,19 +1,28 @@ #include "FlightController.hpp" #include "GSON.h" #include "RF/BluetoothDispatcher.hpp" +#include 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> _routes; }; \ No newline at end of file diff --git a/Logic/PID.hpp b/Logic/PID.hpp index 28d1578..d022306 100644 --- a/Logic/PID.hpp +++ b/Logic/PID.hpp @@ -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); diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp index b4b4a17..fc185b6 100644 --- a/RF/BluetoothDispatcher.cpp +++ b/RF/BluetoothDispatcher.cpp @@ -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); diff --git a/Sensors/MPU.hpp b/Sensors/MPU.hpp index e124402..be952bc 100644 --- a/Sensors/MPU.hpp +++ b/Sensors/MPU.hpp @@ -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; diff --git a/main.cpp b/main.cpp index 95821a4..dd2f438 100644 --- a/main.cpp +++ b/main.cpp @@ -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),