Обновить pilot.cpp

pilot.cpp

@@ -1,9 +1,8 @@
 #include "Tasks/Task.h"
 #include <cmath>
 #include <vector>
-
-// Данная функция выполняется только один раз при запуске программы
-// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
+#define ARDUINOJSON_ENABLE_PROGMEM 0
+#include <ArduinoJson.h>
 
 // Настройки
 constexpr float Ki = 0.03f;
@@ -17,20 +16,25 @@ struct WayPoint {
     float z;
 };
 
-std::vector<WayPoint> wayPoints = {
+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},
 };
 
+JsonDocument doc;
+
 void Task_solution::setup(HardwareSerial *a_Serial) {
     debug_serial = a_Serial;
     _Point_Index = 0;
+    Serial2.begin(115200);
 }
 
 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 static_cast<float>(
+        sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2))
+    );
 }
 
 bool isEqual(const float a, const float b, const float accuracy) {
@@ -49,7 +53,7 @@ SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
     a_telemetry.Psi - угол курса [град]
     a_telemetry.Gam - Угол крена [град]
     a_telemetry.Tan - Угол тангажа [град]
-    a_telemetry.V -  Скорость полёта БЛА [м/с]
+    a_telemetry.V - Скорость полёта БЛА [м/с]
     a_telemetry.Vx1 - Продольная скорость [м/с]
     a_telemetry.Vz1 - Поперечная скорость [м/с]
     a_telemetry.Vy1 - Вертикальная скорость [м/с]
@@ -69,13 +73,12 @@ SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
         -degrees(atan2(target_y - a_telemetry.Z, target_x - a_telemetry.L))
     );
 
-
     _ans.Gamma_direct = GammaReg(a_telemetry.Psi, target_psi);
     _ans.Tang_direct = HeightReg(a_telemetry.H, a_telemetry.Vy1, target_height);
 
     const float dist_to_target = distance(a_telemetry.L, a_telemetry.Z, target_x, target_y);
     if (dist_to_target <= waypoint_radius) {
-        // Проверяем расстояние до цели (радиус 5 метров)
+        // Проверяем расстояние до цели
         if (_Point_Index < wayPoints.size() - 1) {
             _Point_Index++;
         }
@@ -107,3 +110,81 @@ float Task_solution::HeightReg(float Yg, float Vy, float Hz) {
     const float Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -pitch_constraint, pitch_constraint);
     return Pitch_direct;
 }
+
+String uartBuffer;
+bool ReceivedNewMessage(std::string &err) {
+    while (Serial.available() > 0) {
+        const char c = Serial.read();
+        uartBuffer += c;
+        if (c == '\n') { // Ожидание конца строки
+            const DeserializationError error = deserializeJson(doc, uartBuffer);
+            uartBuffer = ""; // Очистка буфера
+            if (error.code() != DeserializationError::Ok) {
+                err = error.c_str();
+                return false;
+            }
+            return true;
+        }
+    }
+    return false;
+}
+
+void printDebugMessage(const String &_) {
+}
+
+void ProcessNewMessage(std::string &err) {
+    if (!doc.containsKey("path")) {
+        err = "Missing 'path' key";
+        printDebugMessage(String(err.c_str()));
+        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;
+        }
+
+        // Проверка наличия полей 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});
+    }
+
+    // Проверка на пустой массив
+    if (newWayPoints.empty()) {
+        err = "No waypoints provided";
+        printDebugMessage(String(err.c_str()));
+        return;
+    }
+
+    wayPoints = std::move(newWayPoints);
+    err.clear(); // Успешное выполнение без ошибок
+}