From 995048bfdc073ec989c4b3c44d888a7cda7d5740 Mon Sep 17 00:00:00 2001 From: Goga Coder Date: Sat, 5 Apr 2025 04:18:22 +0700 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=B8=D1=82?= =?UTF-8?q?=D1=8C=20pilot.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- pilot.cpp | 144 +++++++++++++++++++++++++++++------------------------- 1 file changed, 78 insertions(+), 66 deletions(-) diff --git a/pilot.cpp b/pilot.cpp index b63573e..fca0e95 100644 --- a/pilot.cpp +++ b/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 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( - 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( - -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()) { + 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()) { + err = "Required 'path' field"; + printDebugMessage(err); return; } - - // Получение массива точек const auto pointsArray = doc["path"].as(); - if (pointsArray.isNull()) { - err = "'wp' is not an array"; - printDebugMessage(String(err.c_str())); - return; - } - - std::vector newWayPoints; - - // Обход всех элементов массива - for (JsonVariant value : pointsArray) { - auto point = value.as(); - if (point.isNull()) { - err = "Invalid waypoint format"; - printDebugMessage(String(err.c_str())); - return; + for (const auto &p : pointsArray) { + if (!p["x"].is() || !p["y"].is() || !p["z"].is()) { + 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() || !point["z"].is()) { - 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(); + point.z = p["y"].as(); + point.y = p["z"].as(); + 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; } \ No newline at end of file