Обновить pilot.cpp

pilot.cpp

@@ -6,24 +6,29 @@
 
 // Настройки
 constexpr float Ki = 0.03f;
-constexpr float target_height = 65.f;
-constexpr float waypoint_radius = 1.5f;
+constexpr float waypoint_radius = 3.f;
 constexpr float pitch_constraint = 20.f;
 constexpr float yaw_constraint = 20.f;
 
+// https://github.com/GyverLibs/AsyncStream @ 1.1
+// https://github.com/bblanchon/ArduinoJson @ 7.3.1
+
 struct WayPoint {
     float x;
     float z;
+    float y;
 };
 
-std::vector wayPoints = {
-    WayPoint{.x = -480.f, .z = 85.f},
-    WayPoint{.x = -30.f, .z = 500.f},
-    WayPoint{.x = 150.f, .z = 330.f},
-    WayPoint{.x = 150.f, .z = 5.f},
-};
+std::vector<WayPoint> wayPoints = {};
 
 JsonDocument doc;
+bool cargoDrop = false;
+
+String HandleMessages();
+bool ReceiveNewMessage(String &err);
+void RouteNewMessage(String &err);
+void HandleNewWaypoints(String &err);
+void HandleDropCargo();
 
 void Task_solution::setup(HardwareSerial *a_Serial) {
     debug_serial = a_Serial;
@@ -31,10 +36,11 @@ void Task_solution::setup(HardwareSerial *a_Serial) {
     Serial2.begin(115200);
 }
 
+void printDebugMessage(const String &_) {
+}
+
 float distance(const float x1, const float y1, const float x2, const float y2) {
-    return static_cast<float>(
-        sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2))
-    );
+    return sqrtf(powf(x2 - x1, 2.f) + powf(y2 - y1, 2.f));
 }
 
 bool isEqual(const float a, const float b, const float accuracy) {
@@ -69,8 +75,10 @@ SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
     // _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
     const float target_x = wayPoints[_Point_Index].x;
     const float target_y = wayPoints[_Point_Index].z;
+    const float target_height = wayPoints[_Point_Index].y;
+
     const auto target_psi = static_cast<float>(
-        -degrees(atan2(target_y - a_telemetry.Z, target_x - a_telemetry.L))
+        -degrees(atan2f(target_y - a_telemetry.Z, target_x - a_telemetry.L))
     );
 
     _ans.Gamma_direct = GammaReg(a_telemetry.Psi, target_psi);
@@ -84,6 +92,17 @@ SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
         }
     }
 
+    if (const String err = HandleMessages(); !err.isEmpty()) {
+        printtoDebugSerial(err);
+    }
+
+    if (cargoDrop) {
+        // Propagate changed state
+        cargo_drop = cargoDrop;
+        cargoDrop = false;
+    }
+
+
     // Отправляем команды на симулятор
     // БЛА будет пытаться выдерживать заданные углы крена и тангажа
     return _ans;
@@ -111,12 +130,23 @@ float Task_solution::HeightReg(float Yg, float Vy, float Hz) {
     return Pitch_direct;
 }
 
+
+
+String HandleMessages() {
+    String err;
+    if (ReceiveNewMessage(err)) {
+        RouteNewMessage(err);
+    }
+    return err;
+}
+
 String uartBuffer;
-bool ReceivedNewMessage(std::string &err) {
+bool ReceiveNewMessage(String &err) {
     while (Serial.available() > 0) {
-        const char c = Serial.read();
-        uartBuffer += c;
-        if (c == '\n') { // Ожидание конца строки
+        if (const char c = Serial.read(); c == '\n') { // Ожидание конца строки
+            if (uartBuffer.length() <= 1) {
+                printDebugMessage("Invalid message");
+            }
             const DeserializationError error = deserializeJson(doc, uartBuffer);
             uartBuffer = ""; // Очистка буфера
             if (error.code() != DeserializationError::Ok) {
@@ -124,67 +154,49 @@ bool ReceivedNewMessage(std::string &err) {
                 return false;
             }
             return true;
+        } else {
+            uartBuffer += c;
         }
     }
     return false;
 }
 
-void printDebugMessage(const String &_) {
+void RouteNewMessage(String &err) {
+    if (!doc["cmd"].is<String>()) {
+        err = "Required 'cmd' field";
+        printDebugMessage(err);
+        return;
+    }
+
+   if (doc["cmd"] == "add_wp") {
+       HandleNewWaypoints(err);
+   } else if (doc["cmd"] == "drop") {
+       HandleDropCargo();
+   } else {
+       err = "Unknown command";
+   }
 }
 
-void ProcessNewMessage(std::string &err) {
-    if (!doc.containsKey("path")) {
-        err = "Missing 'path' key";
-        printDebugMessage(String(err.c_str()));
+void HandleNewWaypoints(String &err) {
+    if (!doc["path"].is<JsonArray>()) {
+        err = "Required 'path' field";
+        printDebugMessage(err);
         return;
     }
-
-    // Получение массива точек
     const auto pointsArray = doc["path"].as<JsonArray>();
-    if (pointsArray.isNull()) {
-        err = "'wp' is not an array";
-        printDebugMessage(String(err.c_str()));
-        return;
-    }
-
-    std::vector<WayPoint> newWayPoints;
-
-    // Обход всех элементов массива
-    for (JsonVariant value : pointsArray) {
-        auto point = value.as<JsonObject>();
-        if (point.isNull()) {
-            err = "Invalid waypoint format";
-            printDebugMessage(String(err.c_str()));
-            return;
+    for (const auto &p : pointsArray) {
+        if (!p["x"].is<float>() || !p["y"].is<float>() || !p["z"].is<float>()) {
+            err = "Missing point field";
+            continue;
         }
-
-        // Проверка наличия полей x и z
-        if (!point.containsKey("x") || !point.containsKey("z")) {
-            err = "Waypoint missing x or z field";
-            printDebugMessage(String(err.c_str()));
-            return;
-        }
-
-        // Проверка типов данных
-        if (!point["x"].is<float>() || !point["z"].is<float>()) {
-            err = "Invalid x or z data type";
-            printDebugMessage(String(err.c_str()));
-            return;
-        }
-
-        // Извлечение значений
-        float x = point["x"];
-        float z = point["z"];
-        newWayPoints.push_back(WayPoint{.x = x, .z = z});
+        WayPoint point{};
+        point.x = p["x"].as<float>();
+        point.z = p["y"].as<float>();
+        point.y = p["z"].as<float>();
+        wayPoints.push_back(point);
     }
+}
 
-    // Проверка на пустой массив
-    if (newWayPoints.empty()) {
-        err = "No waypoints provided";
-        printDebugMessage(String(err.c_str()));
-        return;
-    }
-
-    wayPoints = std::move(newWayPoints);
-    err.clear(); // Успешное выполнение без ошибок
+void HandleDropCargo() {
+    cargoDrop = true;
 }