Better routing

Logic/FlightController.cpp

@@ -1,13 +1,16 @@
 #include "FlightController.hpp"
 
-FlightController::FlightController(Sensors *sensors, PIDController *pid1,
+FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
-                                   PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
+                                   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) {
+        if (_status == DeviceStatus::IsFlying) {
-            // TODO: board the device
+            startBoarding();
-            _status = DeviceStatus::IsBoarding;
         } 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;
 }

Logic/FlightController.hpp

@@ -18,24 +18,43 @@ struct ReportedDeviceState {
 };
 
 struct FlightParams {
-    float height;         // [cm]
+    float height;  // [cm]
-    float yaw;      // [degrees]
+    float yaw;     // [degrees]
-    float pitch;  // [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();
+    [[deprecated]] void _takeoff();
-    void _boarding();
+    [[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
 };

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);
+    if (!pkg.parse(package, _jsonMaxDepth)) {
-    // TODO: process package
+        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() {

Logic/FlightDispatcher.hpp

@@ -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);
+    /* Telemetry flow */
-    void _onNewMessageReceived(char *package);
     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;
 };

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

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

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;

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