From fc3506b224e68da331329ee1142027b5c211af3e Mon Sep 17 00:00:00 2001 From: blinker Date: Thu, 25 Jun 2026 18:23:46 +0000 Subject: [PATCH 01/10] [FEATURE] hw-11-uart: added drone_link.hpp, change dockerfile and apt-packages - add libgpiod-dev and others, add it in cmakelists --- .devcontainer/Dockerfile | 3 + .devcontainer/apt-packages.in | 8 ++ drone_simulation/CMakeLists.txt | 7 +- drone_simulation/include/drone_link.hpp | 168 ++++++++++++++++++++++++ 4 files changed, 184 insertions(+), 2 deletions(-) create mode 100644 drone_simulation/include/drone_link.hpp diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 57b4765..0a7786b 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -17,6 +17,9 @@ FROM ubuntu:noble@sha256:c4a8d5503dfb2a3eb8ab5f807da5bc69a85730fb49b5cfca2330194 # інтерактивного stdin немає - без цього build може зависнути назавжди. ARG DEBIAN_FRONTEND="noninteractive" + +RUN dpkg --add-architecture arm64 && apt update +RUN apt install --no-install-recommends -y libgpiod-dev:arm64 # Пакети зі списку apt-packages.in (по одному на рядок, коментарі через #). # # Прапори й чистка: diff --git a/.devcontainer/apt-packages.in b/.devcontainer/apt-packages.in index e55eca6..650c1e6 100644 --- a/.devcontainer/apt-packages.in +++ b/.devcontainer/apt-packages.in @@ -32,3 +32,11 @@ cmake clang-format cmake-format ninja-build + + +# work with rpi pins +libgpiod-dev +socat + +pkg-config +file \ No newline at end of file diff --git a/drone_simulation/CMakeLists.txt b/drone_simulation/CMakeLists.txt index 0c0a32d..8aacdeb 100644 --- a/drone_simulation/CMakeLists.txt +++ b/drone_simulation/CMakeLists.txt @@ -21,11 +21,14 @@ add_library(drone_simulation_lib find_package(Threads REQUIRED) target_link_libraries(drone_simulation_lib PUBLIC Threads::Threads) +find_package(PkgConfig REQUIRED) +pkg_check_modules(GPIOD REQUIRED libgpiod) + target_include_directories(drone_simulation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_compile_options(drone_simulation_lib PRIVATE -Wall -Wextra -pedantic) add_executable(drone_simulation_check src/main.cpp) -target_link_libraries(drone_simulation_check PRIVATE drone_simulation_lib) +target_link_libraries(drone_simulation_check PRIVATE drone_simulation_lib ${GPIOD_LIBRARIES}) +target_include_directories(drone_simulation_check PRIVATE ${GPIOD_INCLUDE_DIRS}) target_compile_options(drone_simulation_check PRIVATE -Wall -Wextra -pedantic) file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/data/ diff --git a/drone_simulation/include/drone_link.hpp b/drone_simulation/include/drone_link.hpp new file mode 100644 index 0000000..f8247e4 --- /dev/null +++ b/drone_simulation/include/drone_link.hpp @@ -0,0 +1,168 @@ +// drone_link.h — бінарний UART-протокол ДЗ11 (заняття 4.4) +// Спільний для симулятора фізики, чекера і програми студента. +// +// Кадр на дроті: +// [0] MAGIC0 = 0xA5 +// [1] MAGIC1 = 0x5A +// [2] TYPE (1 байт) — тип пакета (PacketType) +// [3] LEN (1 байт) — довжина payload у байтах +// [4..4+LEN-1] payload — корисні дані (little-endian) +// [..] CRC16 (2 байти, LE) — CRC-16/CCITT-FALSE по TYPE+LEN+payload +// +// Усі multi-byte поля — little-endian (як на ARM/x86 за замовчуванням). +// Кадр самосинхронізується: приймач шукає MAGIC0,MAGIC1, потім читає LEN і CRC. + +#ifndef DRONE_LINK_H +#define DRONE_LINK_H + +#include +#include + +namespace dlink { + +constexpr uint8_t MAGIC0 = 0xA5; +constexpr uint8_t MAGIC1 = 0x5A; + +enum PacketType : uint8_t { + PKT_TELEMETRY = 0x01, // стан дрона в момент часу + PKT_TARGET = 0x02, // позиція однієї цілі (шлеться періодично по кожній цілі) + PKT_AMMO = 0x03, // параметри боєприпасу + hitRadius (один раз на старті) + PKT_RESULT = 0x04, // вердикт чекера (HIT/MISS) — лише на реальній малині по UART назад + PKT_CONTROL = 0x05, // КЕРУВАННЯ: студент -> чекер (прискорення і швидкість повороту) +}; + +#pragma pack(push, 1) + +// PKT_TELEMETRY — те, що читає і парсить студент +struct Telemetry { + uint32_t t_ms; // час від старту, мілісекунди (таймстемп) + float x, y; // позиція дрона в площині, метри + float z; // висота (altitude), метри + float vx, vy; // швидкість у площині, м/с + float speed; // модуль горизонтальної швидкості, м/с + float dir; // курс (напрямок польоту), радіани + uint8_t state; // стан стейт-машини (0..4, як у DZ3) +}; + +// PKT_TARGET — позиція цілі «зараз» (ціль може рухатися) +struct TargetPos { + uint8_t id; // індекс цілі + float x, y; // поточна позиція цілі, метри +}; + +// PKT_AMMO — конфіг пострілу (надсилається раз на старті) +struct AmmoCfg { + char name[16]; // напр. "VOG-17" + float mass; // m + float drag; // d + float lift; // l + float hitRadius; // радіус успішного влучання, метри + uint8_t nTargets; // скільки цілей у місії +}; + +// PKT_RESULT — вердикт (зворотний канал на залізі) +struct Result { + uint8_t hit; // 1 = влучив, 0 = промах + uint8_t targetId; // у яку ціль (або 0xFF) + float miss_m; // відстань промаху, метри + uint32_t drop_t_ms; // коли спрацював скид +}; + +// PKT_CONTROL — команда керування дроном (студент шле чекеру кожен такт) +// Нормовані значення; чекер множить на фізичні ліміти дрона (maxAccel, maxTurnRate). +struct Control { + float accel; // прискорення вздовж курсу, [-1..1] (1 = повний газ, -1 = гальмо) + float turnRate; // швидкість повороту, [-1..1] (1 = макс. вліво, -1 = вправо) +}; + +#pragma pack(pop) + +// ---- CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF) ---- +inline uint16_t crc16(const uint8_t* data, size_t len) +{ + uint16_t crc = 0xFFFF; + for (size_t i = 0; i < len; ++i) { + crc ^= (uint16_t)data[i] << 8; + for (int b = 0; b < 8; ++b) + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } + return crc; +} + +// Запакувати кадр у буфер out (повертає кількість записаних байтів). +// out має бути >= 6 + payloadLen. +inline size_t encode(uint8_t type, const void* payload, uint8_t payloadLen, uint8_t* out) +{ + out[0] = MAGIC0; + out[1] = MAGIC1; + out[2] = type; + out[3] = payloadLen; + if (payloadLen && payload) + std::memcpy(out + 4, payload, payloadLen); + uint16_t c = crc16(out + 2, (size_t)payloadLen + 2); // по TYPE+LEN+payload + out[4 + payloadLen] = (uint8_t)(c & 0xFF); + out[4 + payloadLen + 1] = (uint8_t)(c >> 8); + return (size_t)payloadLen + 6; +} + +// Інкрементальний парсер: годуєш байтами, на готовому валідному кадрі feed() повертає true. +// Тримає стан між викликами — зручно читати з UART по шматках. +struct Parser { + enum { S_M0, S_M1, S_TYPE, S_LEN, S_PAYLOAD, S_CRC0, S_CRC1 } st = S_M0; + uint8_t type = 0, len = 0, idx = 0; + uint8_t buf[260]; + uint16_t crc_rx = 0; + + // Повертає true, коли зібрано повний валідний кадр. + // type/payload/payloadLen — вихідні (валідні лише при true). + bool feed(uint8_t byte, uint8_t& outType, uint8_t* outPayload, uint8_t& outLen) + { + switch (st) { + case S_M0: + if (byte == MAGIC0) + st = S_M1; + break; + case S_M1: + st = (byte == MAGIC1) ? S_TYPE : S_M0; + break; + case S_TYPE: + type = byte; + st = S_LEN; + break; + case S_LEN: + len = byte; + idx = 0; + st = len ? S_PAYLOAD : S_CRC0; + break; + case S_PAYLOAD: + buf[idx++] = byte; + if (idx >= len) + st = S_CRC0; + break; + case S_CRC0: + crc_rx = byte; + st = S_CRC1; + break; + case S_CRC1: { + crc_rx |= (uint16_t)byte << 8; + st = S_M0; + // перерахувати CRC по TYPE+LEN+payload + uint8_t tmp[262]; + tmp[0] = type; + tmp[1] = len; + std::memcpy(tmp + 2, buf, len); + if (crc16(tmp, (size_t)len + 2) == crc_rx) { + outType = type; + outLen = len; + std::memcpy(outPayload, buf, len); + return true; + } + break; // биті дані — кадр відкинуто, синхронізуємось далі + } + } + return false; + } +}; + +} // namespace dlink +#endif // DRONE_LINK_H From 028fe999e3aeb53597281096d66932c74dbcaebb Mon Sep 17 00:00:00 2001 From: blinker Date: Thu, 25 Jun 2026 19:46:44 +0000 Subject: [PATCH 02/10] [FEATURE] hw11-uart: implemented uart_port (open, read, write) and gpio controller (open, send start and drop signals) --- drone_simulation/CMakeLists.txt | 6 +- .../port_controllers/gpio_controller.hpp | 22 ++++++ .../include/port_controllers/uart_port.hpp | 20 ++++++ .../src/port_controllers/gpio_controller.cpp | 70 +++++++++++++++++++ .../src/port_controllers/uart_port.cpp | 70 +++++++++++++++++++ 5 files changed, 186 insertions(+), 2 deletions(-) create mode 100644 drone_simulation/include/port_controllers/gpio_controller.hpp create mode 100644 drone_simulation/include/port_controllers/uart_port.hpp create mode 100644 drone_simulation/src/port_controllers/gpio_controller.cpp create mode 100644 drone_simulation/src/port_controllers/uart_port.cpp diff --git a/drone_simulation/CMakeLists.txt b/drone_simulation/CMakeLists.txt index 8aacdeb..9c85ff5 100644 --- a/drone_simulation/CMakeLists.txt +++ b/drone_simulation/CMakeLists.txt @@ -16,15 +16,17 @@ add_library(drone_simulation_lib src/drone_states/state_turning.cpp src/drone_states/state_moving.cpp src/drone_physics.cpp + src/port_controllers/uart_port.cpp + src/port_controllers/gpio_controller.cpp ) find_package(Threads REQUIRED) -target_link_libraries(drone_simulation_lib PUBLIC Threads::Threads) +target_link_libraries(drone_simulation_lib PUBLIC Threads::Threads ${GPIOD_LIBRARIES}) find_package(PkgConfig REQUIRED) pkg_check_modules(GPIOD REQUIRED libgpiod) -target_include_directories(drone_simulation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_include_directories(drone_simulation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include ${GPIOD_INCLUDE_DIRS}) add_executable(drone_simulation_check src/main.cpp) target_link_libraries(drone_simulation_check PRIVATE drone_simulation_lib ${GPIOD_LIBRARIES}) diff --git a/drone_simulation/include/port_controllers/gpio_controller.hpp b/drone_simulation/include/port_controllers/gpio_controller.hpp new file mode 100644 index 0000000..41a688e --- /dev/null +++ b/drone_simulation/include/port_controllers/gpio_controller.hpp @@ -0,0 +1,22 @@ +#pragma once +#include // libgpiod — Linux API для роботи з GPIO + +class GpioController { +public: + // chipName — ім'я GPIO контролера: "gpiochip0" (Raspberry Pi) або "gpiochip1" (симуляція) + // startLine — номер піна для сигналу START (24 за замовчуванням) + // dropLine — номер піна для сигналу DROP (23 за замовчуванням) + GpioController(const char* chipName, int startLine, int dropLine); + ~GpioController(); + + // Підняти START у 1 — сигнал чекеру "я готовий, починай симуляцію" + auto signalStart() -> void; + + // Імпульс DROP: 1 → чекати 80мс → 0. Чекер ловить фронт і рахує балістику. + auto signalDrop() -> void; + +private: + gpiod_chip* chip; // "чіп" — контролер, що містить набір GPIO ліній + gpiod_line* startLine; // конкретна лінія (пін) для START + gpiod_line* dropLine; // конкретна лінія (пін) для DROP +}; \ No newline at end of file diff --git a/drone_simulation/include/port_controllers/uart_port.hpp b/drone_simulation/include/port_controllers/uart_port.hpp new file mode 100644 index 0000000..366fa81 --- /dev/null +++ b/drone_simulation/include/port_controllers/uart_port.hpp @@ -0,0 +1,20 @@ +#pragma once +#include +#include + +class UartPort { +public: + // device — шлях до порту: "/tmp/ttyA" (симуляція) або "/dev/ttyAMA1" (Raspberry Pi) + explicit UartPort(const char* device); + ~UartPort(); + + // Прочитати доступні байти з порту. Повертає кількість прочитаних байтів, + // або -1 якщо даних поки нема (бо порт відкритий з O_NONBLOCK). + auto readBytes(uint8_t* buf, size_t maxLen) -> int; + + // Записати байти в порт — відправити дані на інший кінець. + auto writeBytes(const uint8_t* buf, size_t len) -> void; + +private: + int fd; // file descriptor — "номер" відкритого файлу в Linux +}; \ No newline at end of file diff --git a/drone_simulation/src/port_controllers/gpio_controller.cpp b/drone_simulation/src/port_controllers/gpio_controller.cpp new file mode 100644 index 0000000..68048b0 --- /dev/null +++ b/drone_simulation/src/port_controllers/gpio_controller.cpp @@ -0,0 +1,70 @@ +#include "port_controllers/gpio_controller.hpp" +#include // usleep() +#include + +GpioController::GpioController(const char* chipName, int startLine, int dropLine) +{ + // GPIO в Linux організовані в "чіпи" — кожен чіп має набір ліній (пінів). + // На Raspberry Pi це gpiochip0 з ~30 лініями. + // В симуляції gpio-sim створює свій чіп (gpiochip1, gpiochip2...). + // + // gpiod_chip_open_by_name() відкриває /dev/gpiochipN. + this->chip = gpiod_chip_open_by_name(chipName); + + if (!this->chip) { + throw std::runtime_error(std::string("Cannot open GPIO chip: ") + chipName); + } + + // Отримати конкретну лінію за номером. + // Лінія — це один GPIO пін. Номер відповідає фізичному або логічному піну. + this->startLine = gpiod_chip_get_line(chip, startLine); + this->dropLine = gpiod_chip_get_line(chip, dropLine); + + if (!this->startLine || !this->dropLine) { + throw std::runtime_error("Cannot get GPIO lines"); + } + + // "Зарезервувати" лінію як OUTPUT. + // В Linux лінією може керувати тільки один процес. + // gpiod_line_request_output() робить три речі: + // 1. Резервує лінію за нашим процесом (інші не зможуть її змінити) + // 2. Встановлює напрямок — output (ми керуємо, а не читаємо) + // 3. Виставляє початкове значення — 0 (LOW) + // "drone" — ім'я споживача, видно в gpioinfo для відладки. + gpiod_line_request_output(this->startLine, "drone", 0); + gpiod_line_request_output(this->dropLine, "drone", 0); +} + +GpioController::~GpioController() +{ + // Звільнити лінії — інші процеси зможуть їх використовувати. + if (this->startLine) { + gpiod_line_release(this->startLine); + } + + if (this->dropLine) { + gpiod_line_release(this->dropLine); + } + + // Закрити чіп — звільнити file descriptor. + if (this->chip) { + gpiod_chip_close(this->chip); + } +} + +auto GpioController::signalStart() -> void +{ + // Виставити START = 1 (HIGH, 3.3V на реальній платі). + // Чекер постійно читає цю лінію. Щойно бачить 1 — починає симуляцію: + // шле PKT_AMMO, потім кожен такт PKT_TELEMETRY і PKT_TARGET. + // Тримаємо START = 1 до кінця програми. + + gpiod_line_set_value(this->startLine, 1); +} + +auto GpioController::signalDrop() -> void +{ + gpiod_line_set_value(this->dropLine, 1); + usleep(80000); + gpiod_line_set_value(this->dropLine, 0); +} \ No newline at end of file diff --git a/drone_simulation/src/port_controllers/uart_port.cpp b/drone_simulation/src/port_controllers/uart_port.cpp new file mode 100644 index 0000000..7def4b4 --- /dev/null +++ b/drone_simulation/src/port_controllers/uart_port.cpp @@ -0,0 +1,70 @@ +#include // open(), O_RDWR, O_NOCTTY, O_NONBLOCK +#include // termios, cfmakeraw, cfsetispeed, tcsetattr +#include // read(), write(), close() +#include +#include +#include "port_controllers/uart_port.hpp" + +UartPort::UartPort(const char* device) +{ + // Відкриваємо порт як файл. + // O_RDWR — і читати, і писати + // O_NOCTTY — не робити цей порт контрольним терміналом процесу + // (інакше Ctrl+C з порту вб'є програму) + // O_NONBLOCK — read() не блокує: якщо даних нема, одразу повертає -1 + // замість того щоб чекати. Це важливо для головного циклу — + // ми не хочемо зависати на read(), бо потрібно і читати, і слати. + this->fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (this->fd < 0) { + throw std::runtime_error("Cannot open UART: " + std::string(device) + "\n"); + } + + // termios — структура, що описує налаштування послідовного порту. + // Кожен порт має свої параметри: швидкість, кількість біт, парність тощо. + termios tio{}; + tcgetattr(this->fd, &tio); // прочитати поточні налаштування порту + + // cfmakeraw() — вимкнути всю "термінальну" обробку. + // Без цього Linux намагається інтерпретувати дані як текст: + // обробляє Enter, Backspace, Ctrl+C, ехо тощо. + // Нам потрібні сирі байти без жодних перетворень — raw mode. + // Також виставляє 8 біт даних, без парності (8N1). + cfmakeraw(&tio); + + // Швидкість передачі — 115200 біт/с (baud rate). + // Обидва кінці UART мусять мати однакову швидкість, + // інакше дані будуть сміттям. 115200 — стандартна для embedded. + cfsetispeed(&tio, B115200); // вхідна швидкість (receive) + cfsetospeed(&tio, B115200); // вихідна швидкість (transmit) + + // CLOCAL — ігнорувати сигнали модему (DCD/DSR). У нас немає модему. + // CREAD — увімкнути приймач. Без цього read() нічого не поверне. + tio.c_cflag |= (CLOCAL | CREAD); + + // Застосувати налаштування негайно (TCSANOW = "зараз"). + // Альтернатива TCSADRAIN чекала б поки все з буфера відправиться. + tcsetattr(this->fd, TCSANOW, &tio); +} + +UartPort::~UartPort() +{ + if (this->fd >= 0) { + close(this->fd); + } +} + +auto UartPort::readBytes(uint8_t* buf, size_t maxLen) -> int +{ + // read() поверне: + // > 0 — кількість прочитаних байтів + // -1 — помилка або "даних нема" (errno == EAGAIN, бо O_NONBLOCK) + // В головному циклі ми просто ігноруємо -1 і пробуємо знову. + return read(this->fd, buf, maxLen); +} + +auto UartPort::writeBytes(const uint8_t* buf, size_t len) -> void +{ + // write() кладе байти в буфер ядра, звідки вони підуть на дріт. + // Для коротких пакетів (8-16 байт) це відбувається миттєво. + write(this->fd, buf, len); +} \ No newline at end of file From dbbb8f6ea040e6c51c99039ae10230fc9950d680 Mon Sep 17 00:00:00 2001 From: blinker Date: Fri, 26 Jun 2026 22:14:38 +0000 Subject: [PATCH 03/10] [FEATURE] hw11-uart: listen uart port and parse data, send start signal via gpio --- .../include/port_controllers/uart_port.hpp | 4 +- drone_simulation/src/main.cpp | 142 +++++++++++++----- .../src/port_controllers/uart_port.cpp | 8 +- 3 files changed, 108 insertions(+), 46 deletions(-) diff --git a/drone_simulation/include/port_controllers/uart_port.hpp b/drone_simulation/include/port_controllers/uart_port.hpp index 366fa81..a9426be 100644 --- a/drone_simulation/include/port_controllers/uart_port.hpp +++ b/drone_simulation/include/port_controllers/uart_port.hpp @@ -10,10 +10,10 @@ class UartPort { // Прочитати доступні байти з порту. Повертає кількість прочитаних байтів, // або -1 якщо даних поки нема (бо порт відкритий з O_NONBLOCK). - auto readBytes(uint8_t* buf, size_t maxLen) -> int; + auto readBytes(uint8_t* buffer, size_t maxByteCount) -> int; // Записати байти в порт — відправити дані на інший кінець. - auto writeBytes(const uint8_t* buf, size_t len) -> void; + auto writeBytes(const uint8_t* buffer, size_t byteCount) -> void; private: int fd; // file descriptor — "номер" відкритого файлу в Linux diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index 88296ea..37978d5 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -1,11 +1,13 @@ #include #include #include -#include "json.hpp" +#include +#include #include "simulation.hpp" #include "mission_processor.hpp" -#include "config_loaders/factory.hpp" -#include "providers/thread_safe_target_provider.hpp" +#include "drone_link.hpp" +#include "port_controllers/uart_port.hpp" +#include "port_controllers/gpio_controller.hpp" using json = nlohmann::json; @@ -27,48 +29,108 @@ using json = nlohmann::json; auto main(int argc, char* argv[]) -> int { try { - const std::string configPath = (argc > 1) ? argv[1] : "config.json"; - const std::string ammoPath = (argc > 2) ? argv[2] : "ammo.json"; - const std::string targetsPath = (argc > 3) ? argv[3] : "targets.json"; - const std::string solverArg = (argc > 4) ? argv[4] : "analytical"; - const std::string ballisticTablePath = (argc > 5) ? argv[5] : "ballistic_table.txt"; - - auto loader = createLoader(LoaderType::FILE, configPath, ammoPath); - - loader->load(); - DroneConfig droneConfig = loader->getConfig(); - auto physics = std::make_unique(droneConfig); - auto targets = std::make_unique(targetsPath, droneConfig.arrayTimeStep, droneConfig.timeScale); - auto* targetsPtr = targets.get(); - auto* physicsPtr = physics.get(); - - SolverType solverType = (solverArg == "analytical") ? SolverType::ANALYTICAL : SolverType::TABLE; - auto ballisticSolver = createSolver(solverType, droneConfig, ballisticTablePath); - - MissionProcessor missionProcessor(std::move(targets), std::move(ballisticSolver), physicsPtr); - missionProcessor.init(std::move(loader)); - - std::thread providerThread(&ThreadSafeTargetProvider::run, targetsPtr); - std::thread physicsThread(&DronePhysics::run, physicsPtr); - std::thread missionThread(&MissionProcessor::run, &missionProcessor); - - while (!targetsPtr->isThreadReady() || !physicsPtr->isThreadReady() || !missionProcessor.isThreadReady()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::string uartDev = "/tmp/ttyA"; + std::string gpioChip = "gpiochip0"; + int startLine = 24; + int dropLine = 23; + + for (int i = 1; i < argc; i++) { + std::string arg = argv[i]; + if (arg == "--uart" && i + 1 < argc) { + uartDev = argv[++i]; + } + else if (arg == "--gpiochip" && i + 1 < argc) { + gpioChip = argv[++i]; + } + else if (arg == "--start-line" && i + 1 < argc) { + startLine = std::stoi(argv[++i]); + } + else if (arg == "--drop-line" && i + 1 < argc) { + dropLine = std::stoi(argv[++i]); + } } - targetsPtr->start(); - physicsPtr->start(); - missionProcessor.start(); - - missionThread.join(); + std::cout << "UART: " << uartDev << " GPIO: " << gpioChip << " START: " << startLine << " DROP: " << dropLine << std::endl; + + UartPort uart(uartDev.c_str()); + GpioController gpio(gpioChip.c_str(), startLine, dropLine); + + gpio.signalStart(); + std::cout << "START signal sent, waiting for data..." << std::endl; + + dlink::AmmoCfg ammo{}; + bool ammoReceived = false; + + std::vector targetPositions; + int targetCount = 0; + + dlink::Telemetry telemetry{}; + + bool dropped = false; + + dlink::Parser parser; + uint8_t buffer[256]; + + while (!dropped) { + int n = uart.readBytes(buffer, sizeof(buffer)); + + if (n <= 0) { + continue; + } + + uint8_t type, len, payload[260]; + for (int i = 0; i < n; i++) { + // feed() повертає true коли зібрано повний валідний кадр + if (!parser.feed(buffer[i], type, payload, len)) { + continue; + } + + switch (type) { + case dlink::PKT_TELEMETRY: { + std::memcpy(&telemetry, payload, sizeof(telemetry)); + + std::cout << "t=" << telemetry.t_ms << " pos=(" << telemetry.x << "," << telemetry.y << ")" << " speed=" << telemetry.speed + << " dir=" << telemetry.dir << std::endl; + + dlink::Control control{0.0f, 0.0f}; + uint8_t out[64]; + size_t m = encode(dlink::PKT_CONTROL, &control, sizeof(control), out); + uart.writeBytes(out, m); + break; + } + + case dlink::PKT_AMMO: { + // Приходить один раз на старті. + // Містить параметри боєприпасу і кількість цілей. + std::memcpy(&ammo, payload, sizeof(ammo)); + ammoReceived = true; + targetCount = ammo.nTargets; + std::cout << "AMMO: " << ammo.name << " mass=" << ammo.mass << " drag=" << ammo.drag << " lift=" << ammo.lift + << " hitRadius=" << ammo.hitRadius << " targets=" << (int)ammo.nTargets << std::endl; + break; + } + + case dlink::PKT_TARGET: { + // Приходить періодично для кожної цілі. + // Оновлюємо позицію за id. + dlink::TargetPos targetPosition; + std::memcpy(&targetPosition, payload, sizeof(targetPosition)); + if (targetPosition.id < targetPositions.size()) { + targetPositions[targetPosition.id] = targetPosition; + } + break; + } + } + } + } - physicsPtr->stop(); - targetsPtr->stop(); + // SolverType solverType = (solverArg == "analytical") ? SolverType::ANALYTICAL : SolverType::TABLE; + // auto ballisticSolver = createSolver(solverType, droneConfig, ballisticTablePath); - physicsThread.join(); - providerThread.join(); + // MissionProcessor missionProcessor(std::move(targets), std::move(ballisticSolver), physicsPtr); + // missionProcessor.init(std::move(loader)); - writeSimulationJSONFile(missionProcessor.getSteps()); + // writeSimulationJSONFile(missionProcessor.getSteps()); return 0; } diff --git a/drone_simulation/src/port_controllers/uart_port.cpp b/drone_simulation/src/port_controllers/uart_port.cpp index 7def4b4..b8d9c28 100644 --- a/drone_simulation/src/port_controllers/uart_port.cpp +++ b/drone_simulation/src/port_controllers/uart_port.cpp @@ -53,18 +53,18 @@ UartPort::~UartPort() } } -auto UartPort::readBytes(uint8_t* buf, size_t maxLen) -> int +auto UartPort::readBytes(uint8_t* buffer, size_t maxByteCount) -> int { // read() поверне: // > 0 — кількість прочитаних байтів // -1 — помилка або "даних нема" (errno == EAGAIN, бо O_NONBLOCK) // В головному циклі ми просто ігноруємо -1 і пробуємо знову. - return read(this->fd, buf, maxLen); + return read(this->fd, buffer, maxByteCount); } -auto UartPort::writeBytes(const uint8_t* buf, size_t len) -> void +auto UartPort::writeBytes(const uint8_t* buffer, size_t byteCount) -> void { // write() кладе байти в буфер ядра, звідки вони підуть на дріт. // Для коротких пакетів (8-16 байт) це відбувається миттєво. - write(this->fd, buf, len); + write(this->fd, buffer, byteCount); } \ No newline at end of file From ecb6d663e92a6114584bbada6b2f6fd7db8366b2 Mon Sep 17 00:00:00 2001 From: blinker Date: Sat, 27 Jun 2026 11:20:55 +0000 Subject: [PATCH 04/10] [FEATURE] hw11-uart: fix MP, to use telemetry from uart, use hardcoded accelPath and attack speed, need to clarify it --- .../include/mission_processor.hpp | 51 ++-- drone_simulation/include/simulation.hpp | 11 +- drone_simulation/src/main.cpp | 72 ++--- drone_simulation/src/mission_processor.cpp | 277 ++++++++---------- drone_simulation/src/simulation.cpp | 26 ++ 5 files changed, 207 insertions(+), 230 deletions(-) diff --git a/drone_simulation/include/mission_processor.hpp b/drone_simulation/include/mission_processor.hpp index bc79998..af25d16 100644 --- a/drone_simulation/include/mission_processor.hpp +++ b/drone_simulation/include/mission_processor.hpp @@ -1,48 +1,35 @@ #pragma once -#include -#include #include #include "interfaces/ballistic_solver.hpp" -#include "interfaces/config_loader.hpp" -#include "interfaces/target_provider.hpp" -#include "interfaces/drone_state.hpp" -#include "drone_physics.hpp" +#include "simulation.hpp" class MissionProcessor { public: - MissionProcessor(std::unique_ptr targetProvider, std::unique_ptr solver, DronePhysics* physics); + explicit MissionProcessor(std::unique_ptr solver); - auto init(std::unique_ptr configLoader) -> void; - auto getCurrentStep() -> int; - auto step() -> SimulationStep; + auto init(const dlink::AmmoCfg& ammoCfg, float altitude, float attackSpd) -> void; + auto process(const dlink::Telemetry& telem, const std::vector& targets, int targetCount) -> MissionResult; auto reset() -> void; auto changeSolver(std::unique_ptr ballisticSolver) -> void; - auto run() -> void; - auto start() -> void; - auto isThreadReady() const -> bool; - auto getSteps() const -> const std::vector&; private: - auto evaluateTarget(int targetIndex, const DroneTelemetry& telemetry, Coord& outFireCoord) -> float; - auto getSimulationStep(int targetIndex, const Coord& fireCoord, const DroneTelemetry& telemetry) -> SimulationStep; - auto hasNext() -> bool; + auto evaluateTarget( + int targetIndex, const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity, Coord& outFireCoord) -> float; + auto checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos) -> bool; + auto computeControl(const DroneTelemetry& telemetry, const Coord& firePoint) -> dlink::Control; - const int MAX_STEPS = 10000; - std::unique_ptr targets; std::unique_ptr ballisticSolver; - DroneConfig droneConfig; AmmoParams ammo; - int currentStep = 1; int prevTargetIndex = -1; - float currentTime = 0.0f; - float ammoFlightTime; - float ammoHorizontalFlightDistance; - float acceleration; - float prevDirectionToTarget = 0.0f; - bool hit = false; - DronePhysics* dronePhysics; - std::atomic ready{false}; - std::atomic running{false}; - std::vector simulationSteps; - std::unique_ptr droneState; + float ammoFlightTime = 0.0f; + float ammoHorizontalFlightDistance = 0.0f; + float acceleration = 0.0f; + float attackSpeed = 0.0f; + float hitRadius = 0.0f; + bool initialized = false; + + // Для обчислення швидкості цілей (чекер шле тільки позицію, не швидкість) + std::vector prevTargetPositions; + std::vector targetVelocities; + float prevTime = -1.0f; }; diff --git a/drone_simulation/include/simulation.hpp b/drone_simulation/include/simulation.hpp index 3a3ac75..93b73fc 100644 --- a/drone_simulation/include/simulation.hpp +++ b/drone_simulation/include/simulation.hpp @@ -4,6 +4,7 @@ #include #include #include "json.hpp" +#include "drone_link.hpp" using json = nlohmann::json; @@ -119,6 +120,12 @@ struct DroneTelemetry { float timeSecSinceStart; }; +struct MissionResult { + dlink::Control control; + bool shouldDrop; + int bestTargetIndex; +}; + float getDistanceToTarget(const Coord& target, const Coord& posiiton); Coord getFireCoords(const Coord& targetCoord, const Coord& dronePos, const float& ratio); @@ -162,4 +169,6 @@ float getAmmoFlightTime(const DroneConfig& droneConfig, const AmmoParams& ammo); float length(const Coord& coord); -Coord normalize(const Coord& coord); \ No newline at end of file +Coord normalize(const Coord& coord); + +dlink::Control computeControl(const dlink::Telemetry& telem, const dlink::TargetPos& target); \ No newline at end of file diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index 37978d5..5621131 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -1,31 +1,13 @@ #include #include #include -#include #include -#include "simulation.hpp" +#include #include "mission_processor.hpp" -#include "drone_link.hpp" +#include "solvers/analytical_solver.hpp" #include "port_controllers/uart_port.hpp" #include "port_controllers/gpio_controller.hpp" -using json = nlohmann::json; - -#define ENABLE_LOG 1 -#define ENABLE_DEBUG 0 - -#if ENABLE_LOG -#define LOG(msg) std::cout << "[LOG] " << msg << std::endl -#else -#define LOG(msg) -#endif - -#if ENABLE_DEBUG -#define DEBUG(msg) std::cout << "[DEBUG] " << msg << std::endl -#else -#define DEBUG(msg) -#endif - auto main(int argc, char* argv[]) -> int { try { @@ -67,6 +49,9 @@ auto main(int argc, char* argv[]) -> int dlink::Telemetry telemetry{}; bool dropped = false; + bool mpInitialized = false; + + std::unique_ptr missionProcessor; dlink::Parser parser; uint8_t buffer[256]; @@ -80,7 +65,6 @@ auto main(int argc, char* argv[]) -> int uint8_t type, len, payload[260]; for (int i = 0; i < n; i++) { - // feed() повертає true коли зібрано повний валідний кадр if (!parser.feed(buffer[i], type, payload, len)) { continue; } @@ -92,29 +76,54 @@ auto main(int argc, char* argv[]) -> int std::cout << "t=" << telemetry.t_ms << " pos=(" << telemetry.x << "," << telemetry.y << ")" << " speed=" << telemetry.speed << " dir=" << telemetry.dir << std::endl; + // Створити солвер і MP після першої телеметрії — тепер знаємо altitude і speed + if (ammoReceived && !mpInitialized) { + float speed = telemetry.speed > 1.0f ? telemetry.speed : 30.0f; + + DroneConfig solverConfig{}; + solverConfig.altitude = telemetry.z; + solverConfig.attackSpeed = speed; + + auto solver = std::make_unique(solverConfig); + missionProcessor = std::make_unique(std::move(solver)); + missionProcessor->init(ammo, telemetry.z, speed); + mpInitialized = true; + } + + // Отримати рішення від MP dlink::Control control{0.0f, 0.0f}; + if (mpInitialized) { + MissionResult result = missionProcessor->process(telemetry, targetPositions, targetCount); + + if (result.shouldDrop && !dropped) { + std::cout << "DROP!" << std::endl; + gpio.signalDrop(); + dropped = true; + } + + control = result.control; + } + uint8_t out[64]; - size_t m = encode(dlink::PKT_CONTROL, &control, sizeof(control), out); + size_t m = dlink::encode(dlink::PKT_CONTROL, &control, sizeof(control), out); uart.writeBytes(out, m); break; } case dlink::PKT_AMMO: { - // Приходить один раз на старті. - // Містить параметри боєприпасу і кількість цілей. std::memcpy(&ammo, payload, sizeof(ammo)); ammoReceived = true; targetCount = ammo.nTargets; + targetPositions.resize(targetCount); std::cout << "AMMO: " << ammo.name << " mass=" << ammo.mass << " drag=" << ammo.drag << " lift=" << ammo.lift << " hitRadius=" << ammo.hitRadius << " targets=" << (int)ammo.nTargets << std::endl; break; } case dlink::PKT_TARGET: { - // Приходить періодично для кожної цілі. - // Оновлюємо позицію за id. dlink::TargetPos targetPosition; std::memcpy(&targetPosition, payload, sizeof(targetPosition)); + if (targetPosition.id < targetPositions.size()) { targetPositions[targetPosition.id] = targetPosition; } @@ -124,19 +133,10 @@ auto main(int argc, char* argv[]) -> int } } - // SolverType solverType = (solverArg == "analytical") ? SolverType::ANALYTICAL : SolverType::TABLE; - // auto ballisticSolver = createSolver(solverType, droneConfig, ballisticTablePath); - - // MissionProcessor missionProcessor(std::move(targets), std::move(ballisticSolver), physicsPtr); - // missionProcessor.init(std::move(loader)); - - // writeSimulationJSONFile(missionProcessor.getSteps()); - return 0; } catch (const std::exception& error) { - LOG("Error: " << error.what()); - + std::cerr << "Error: " << error.what() << std::endl; return 1; } } diff --git a/drone_simulation/src/mission_processor.cpp b/drone_simulation/src/mission_processor.cpp index f00d6ad..50ff3bc 100644 --- a/drone_simulation/src/mission_processor.cpp +++ b/drone_simulation/src/mission_processor.cpp @@ -1,44 +1,80 @@ #include +#include +#include #include "mission_processor.hpp" -#include "drone_states/state_stopped.hpp" - -MissionProcessor::MissionProcessor(std::unique_ptr targetProvider, - std::unique_ptr solver, - DronePhysics* physics) - : targets(std::move(targetProvider)) - , ballisticSolver(std::move(solver)) - , dronePhysics(physics) + +MissionProcessor::MissionProcessor(std::unique_ptr solver) + : ballisticSolver(std::move(solver)) { - this->droneState = std::make_unique(); } -auto MissionProcessor::init(std::unique_ptr configLoader) -> void +auto MissionProcessor::init(const dlink::AmmoCfg& ammoCfg, float altitude, float attackSpd) -> void { - configLoader->load(); - this->droneConfig = configLoader->getConfig(); - this->ammo = configLoader->getAmmoParams(); + this->ammo = {.name = std::string(ammoCfg.name), .mass = ammoCfg.mass, .drag = ammoCfg.drag, .lift = ammoCfg.lift}; + this->hitRadius = ammoCfg.hitRadius; + this->attackSpeed = attackSpd; - this->ammoFlightTime = getAmmoFlightTime(this->droneConfig, this->ammo); - this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->droneConfig.attackSpeed, this->ammoFlightTime, this->ammo); - this->acceleration = this->droneConfig.attackSpeed * this->droneConfig.attackSpeed / (2.0f * this->droneConfig.accelPath); -} + float accelPath = 50.0f; + this->acceleration = this->attackSpeed * this->attackSpeed / (2.0f * accelPath); -auto MissionProcessor::getCurrentStep() -> int -{ - return this->currentStep; + DroneConfig tempConfig{}; + tempConfig.altitude = altitude; + tempConfig.attackSpeed = this->attackSpeed; + + this->ammoFlightTime = getAmmoFlightTime(tempConfig, this->ammo); + this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->attackSpeed, this->ammoFlightTime, this->ammo); + + this->initialized = true; + + std::cout << "MP init: flightTime=" << ammoFlightTime << " horizDist=" << ammoHorizontalFlightDistance << std::endl; } -auto MissionProcessor::step() -> SimulationStep +auto MissionProcessor::process(const dlink::Telemetry& telem, + const std::vector& targets, + int targetCount) -> MissionResult { + if (!this->initialized || targetCount <= 0) { + return {dlink::Control{0.0f, 0.0f}, false, -1}; + } + + DroneTelemetry droneTelem = { + .pos = {telem.x, telem.y}, .speed = telem.speed, .direction = telem.dir, .timeSecSinceStart = telem.t_ms / 1000.0f}; + + // Обчислити швидкості цілей з різниці позицій між тактами + float currentTime = telem.t_ms / 1000.0f; + + if (prevTime >= 0.0f && !prevTargetPositions.empty()) { + float dt = currentTime - prevTime; + if (dt > 1e-6f) { + targetVelocities.resize(targetCount, {0.0f, 0.0f}); + for (int i = 0; i < targetCount && i < (int)targets.size() && i < (int)prevTargetPositions.size(); ++i) { + Coord current = {targets[i].x, targets[i].y}; + targetVelocities[i] = (current - prevTargetPositions[i]) / dt; + } + } + } + else { + targetVelocities.resize(targetCount, {0.0f, 0.0f}); + } + + // Зберегти поточні позиції для наступного такту + prevTargetPositions.resize(targetCount); + for (int i = 0; i < targetCount && i < (int)targets.size(); ++i) { + prevTargetPositions[i] = {targets[i].x, targets[i].y}; + } + prevTime = currentTime; + + // Знайти найкращу ціль int bestTargetIndex = -1; float bestTotalTime = INFINITY; - Coord bestFireCoord = {.x = 0, .y = 0}; - DroneTelemetry droneTelemetry = this->dronePhysics->getTelemetry(); + Coord bestFireCoord = {0, 0}; - for (int i = 0; i < this->targets->getTargetCount(); ++i) { + for (int i = 0; i < targetCount && i < (int)targets.size(); ++i) { + Coord targetPos = {targets[i].x, targets[i].y}; + Coord velocity = (i < (int)targetVelocities.size()) ? targetVelocities[i] : Coord{0, 0}; Coord fireCoord; - float totalTime = this->evaluateTarget(i, droneTelemetry, fireCoord); + float totalTime = evaluateTarget(i, droneTelem, targetPos, velocity, fireCoord); if (totalTime < bestTotalTime) { bestTotalTime = totalTime; @@ -48,149 +84,54 @@ auto MissionProcessor::step() -> SimulationStep } if (bestTargetIndex < 0) { - std::cout << "No valid target at this step, skipping" << std::endl; - ++this->currentStep; - this->currentTime += this->droneConfig.simTimeStep; - - SimulationStep emptyStep = {.hit = false, - .target = -1, - .droneSpeed = droneTelemetry.speed, - .droneDirection = droneTelemetry.direction, - .timeSecSinceStart = droneTelemetry.timeSecSinceStart, - .dropPoint = {0, 0}, - .aimPoint = {0, 0}, - .predictedTarget = {0, 0}, - .dronePosition = droneTelemetry.pos, - .droneState = DroneStates::Stopped}; - - return emptyStep; + return {dlink::Control{0.0f, 0.0f}, false, -1}; } - Coord deltaToFire = bestFireCoord - droneTelemetry.pos; - Coord dirVec = normalize(deltaToFire); - float newDirection = atan2(dirVec.y, dirVec.x); + Coord bestTargetPos = {targets[bestTargetIndex].x, targets[bestTargetIndex].y}; + bool shouldDrop = checkDrop(droneTelem, bestTargetPos); - float deltaAngle = normalizeAngle(newDirection - droneTelemetry.direction); - - DroneStateInput stateInput = {.deltaAngle = deltaAngle, .speed = droneTelemetry.speed, .config = this->droneConfig}; - auto nextState = this->droneState->execute(stateInput); - if (nextState) { - this->droneState = std::move(nextState); - } - - SimulationStep simulationStep = this->getSimulationStep(bestTargetIndex, bestFireCoord, droneTelemetry); - simulationStep.timeSecSinceStart = droneTelemetry.timeSecSinceStart; - simulationStep.droneState = this->droneState->name(); - - this->hit = simulationStep.hit; - - float sign = (deltaAngle > 0) ? 1.0f : -1.0f; - float angleSpeed = sign * std::min(std::fabs(deltaAngle) / this->droneConfig.simTimeStep, this->droneConfig.angularSpeed); - - DroneCommand command = {.state = this->droneState->name(), .angleSpeed = angleSpeed}; - this->dronePhysics->sendCommand(command); + dlink::Control control = computeControl(droneTelem, bestFireCoord); this->prevTargetIndex = bestTargetIndex; - this->currentStep++; - this->currentTime += this->droneConfig.simTimeStep; - this->prevDirectionToTarget = newDirection; - - return simulationStep; -} - -auto MissionProcessor::reset() -> void -{ - this->hit = false; - this->prevTargetIndex = -1; - this->currentStep = 1; - this->currentTime = 0.0f; -} - -auto MissionProcessor::changeSolver(std::unique_ptr ballisticSolver) -> void -{ - this->ballisticSolver = std::move(ballisticSolver); -} - -auto MissionProcessor::start() -> void -{ - this->running.store(true); -} - -auto MissionProcessor::isThreadReady() const -> bool -{ - return this->ready.load(); -} -auto MissionProcessor::getSteps() const -> const std::vector& -{ - return this->simulationSteps; + return {control, shouldDrop, bestTargetIndex}; } -void MissionProcessor::run() +auto MissionProcessor::evaluateTarget( + int targetIndex, const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity, Coord& outFireCoord) -> float { - this->ready.store(true); - - while (!this->running.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - this->simulationSteps.push_back({.hit = false, - .target = -1, - .droneDirection = this->droneConfig.initialDir, - .dropPoint = {0, 0}, - .aimPoint = {0, 0}, - .predictedTarget = {0, 0}, - .dronePosition = this->droneConfig.startPos, - .droneState = DroneStates::Stopped}); - - while (this->hasNext()) { - SimulationStep simulationStep = this->step(); - if (simulationStep.target >= 0) { - this->simulationSteps.push_back(simulationStep); - } - std::this_thread::sleep_for(std::chrono::duration(this->droneConfig.simTimeStep / this->droneConfig.timeScale)); - } -} - -auto MissionProcessor::evaluateTarget(int targetIndex, const DroneTelemetry& telemetry, Coord& outFireCoord) -> float -{ - Target t = this->targets->getTarget(targetIndex); - Coord target = t.pos; - - float distanceToTarget = getDistanceToTarget(target, telemetry.pos); + // Прохід 1: грубий — fire point для поточної позиції цілі + float distanceToTarget = getDistanceToTarget(targetPos, telemetry.pos); if (distanceToTarget < 1e-3f) { - std::cout << "Distance to the target " << targetIndex << " is near or less then 0\n" << std::endl; return INFINITY; } - float ratio = getRatio(distanceToTarget, this->ammoHorizontalFlightDistance); - Coord fireCoord = getFireCoords(target, telemetry.pos, ratio); + float ratio = getRatio(distanceToTarget, this->ammoHorizontalFlightDistance); + Coord fireCoord = getFireCoords(targetPos, telemetry.pos, ratio); float distanceToFire = getDistanceToTarget(fireCoord, telemetry.pos); - float droneToFireTime = distanceToFire / this->droneConfig.attackSpeed; + float droneToFireTime = distanceToFire / this->attackSpeed; float totalTimeRough = droneToFireTime + this->ammoFlightTime; - Coord predictedTarget = t.pos + t.velocity * totalTimeRough; + // Прохід 2: точний — передбачити де буде ціль через totalTimeRough + Coord predictedTarget = targetPos + targetVelocity * totalTimeRough; float newDistanceToTarget = getDistanceToTarget(predictedTarget, telemetry.pos); if (newDistanceToTarget < 1e-3f) { - std::cout << "Predicted distance to the target " << targetIndex << " is near or less then 0\n" << std::endl; return INFINITY; } float newRatio = getRatio(newDistanceToTarget, this->ammoHorizontalFlightDistance); outFireCoord = getFireCoords(predictedTarget, telemetry.pos, newRatio); float newDistanceToFire = getDistanceToTarget(outFireCoord, telemetry.pos); - float newDroneToFireTime = newDistanceToFire / this->droneConfig.attackSpeed; + float newDroneToFireTime = newDistanceToFire / this->attackSpeed; float totalTime = newDroneToFireTime + this->ammoFlightTime; + // Штраф за маневр якщо fire point за дроном if (newRatio < 0) { - totalTime += getManeuveringTime(newDistanceToTarget, - this->ammoHorizontalFlightDistance, - this->droneConfig.attackSpeed, - this->acceleration, - this->droneConfig.angularSpeed); + totalTime += getManeuveringTime(newDistanceToTarget, this->ammoHorizontalFlightDistance, this->attackSpeed, this->acceleration, 1.0f); } + // Штраф за зміну цілі if (this->prevTargetIndex != targetIndex && this->prevTargetIndex != -1) { totalTime += telemetry.speed / this->acceleration; } @@ -198,36 +139,50 @@ auto MissionProcessor::evaluateTarget(int targetIndex, const DroneTelemetry& tel return totalTime; } -auto MissionProcessor::getSimulationStep(int targetIndex, const Coord& fireCoord, const DroneTelemetry& telemetry) -> SimulationStep +auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos) -> bool { - Coord bombLandCoord = getBombLandCoord(telemetry, this->ammoHorizontalFlightDistance); + Coord bombLand = getBombLandCoord(telemetry, ammoHorizontalFlightDistance); + float miss = getDistanceToTarget(bombLand, targetPos); - Target target = this->targets->getTarget(targetIndex); - Coord targetAtImpact = target.pos + target.velocity * this->ammoFlightTime; + float dx = targetPos.x - telemetry.pos.x; + float dy = targetPos.y - telemetry.pos.y; + float desiredDir = std::atan2(dy, dx); + float deltaAngle = normalizeAngle(desiredDir - telemetry.direction); - float bombMissDistance = getDistanceToTarget(bombLandCoord, targetAtImpact); - Coord dir = {.x = std::cos(telemetry.direction), .y = std::sin(telemetry.direction)}; - Coord aimPoint = telemetry.pos + dir * this->ammoHorizontalFlightDistance; + return miss <= hitRadius && telemetry.speed > 5.0f && std::fabs(deltaAngle) < 0.3f; +} + +auto MissionProcessor::computeControl(const DroneTelemetry& telemetry, const Coord& firePoint) -> dlink::Control +{ + float dx = firePoint.x - telemetry.pos.x; + float dy = firePoint.y - telemetry.pos.y; + float desiredDir = std::atan2(dy, dx); + float deltaAngle = normalizeAngle(desiredDir - telemetry.direction); - bool isHit = bombMissDistance < this->droneConfig.hitRadius || bombLandCoord == targetAtImpact; + float Kp = 2.0f; + float turnRate = std::clamp(Kp * deltaAngle, -1.0f, 1.0f); - if (isHit) { - std::cout << (bombLandCoord == targetAtImpact ? "Direct hit" : "Drone hit the target. Bomb miss: ") - << (bombLandCoord == targetAtImpact ? "" : std::to_string(bombMissDistance)) << std::endl; + float accel; + if (std::fabs(deltaAngle) > 0.3f) { + accel = 0.2f; + } + else { + accel = 1.0f; } - return {.hit = isHit, - .target = targetIndex, - .droneSpeed = telemetry.speed, - .droneDirection = telemetry.direction, - .dropPoint = fireCoord, - .aimPoint = aimPoint, - .predictedTarget = targetAtImpact, - .dronePosition = telemetry.pos, - .droneState = DroneStates::Stopped}; + return dlink::Control{accel, turnRate}; } -auto MissionProcessor::hasNext() -> bool +auto MissionProcessor::reset() -> void { - return this->currentStep <= this->MAX_STEPS && !this->hit; -} \ No newline at end of file + prevTargetIndex = -1; + prevTime = -1.0f; + prevTargetPositions.clear(); + targetVelocities.clear(); + initialized = false; +} + +auto MissionProcessor::changeSolver(std::unique_ptr ballisticSolver) -> void +{ + this->ballisticSolver = std::move(ballisticSolver); +} diff --git a/drone_simulation/src/simulation.cpp b/drone_simulation/src/simulation.cpp index 302e384..f82e676 100644 --- a/drone_simulation/src/simulation.cpp +++ b/drone_simulation/src/simulation.cpp @@ -278,4 +278,30 @@ float length(const Coord& coord) Coord normalize(const Coord& coord) { return coord / length(coord); +} + +dlink::Control computeControl(const dlink::Telemetry& telemetry, const dlink::TargetPos& target) +{ + float dx = target.x - telemetry.x; + float dy = target.y - telemetry.y; + + float desiredDir = std::atan2(dy, dx); + + float deltaAngle = normalizeAngle(desiredDir - telemetry.dir); + + // turnRate — P-регулятор + // TODO: check if it's needed + float Kp = 2.0f; + float turnRate = std::clamp(Kp * deltaAngle, -1.0f, 1.0f); + + // accel — газ або гальмо залежно від курсу + float accel; + if (std::fabs(deltaAngle) > 0.3f) { + accel = 0.2f; // не розганятись поки не повернулись + } + else { + accel = 1.0f; // курс правильний — повний газ + } + + return dlink::Control{accel, turnRate}; } \ No newline at end of file From adc9dacca82ce17a9d882a4379b8a10dfbad8c06 Mon Sep 17 00:00:00 2001 From: blinker Date: Mon, 29 Jun 2026 10:08:28 +0000 Subject: [PATCH 05/10] [FEATURE] hw11-uart: add DroneController and lead guidance for moving targets - add DroneController module: converts mission decisions into UART CONTROL commands (accel + turnRate) - steer toward the predicted target instead of the collapsing firePoint, removing the atan2 singularity at the drop moment - guidance = pursuit (Kp*deltaAngle) + LOS-rate feed-forward (Kd*losRate, proportional navigation) to kill heading lag against maneuvering targets - couple throttle to heading error: hold attackSpeed when aligned, coast while turning - MissionProcessor: predict target over ammoFlightTime for both aiming (aimPoint) and the drop check, so the bomb leads a moving target - expose aimPoint via MissionResult and wire DroneController into the main loop - remove dead state-machine code carried over from earlier homework --- drone_simulation/CMakeLists.txt | 1 + .../include/mission_processor.hpp | 20 +-- drone_simulation/include/simulation.hpp | 19 ++- .../src/config_loaders/file_config_loader.cpp | 34 ++-- drone_simulation/src/main.cpp | 38 +++-- drone_simulation/src/mission_processor.cpp | 145 ++++++++---------- drone_simulation/src/simulation.cpp | 9 ++ 7 files changed, 137 insertions(+), 129 deletions(-) diff --git a/drone_simulation/CMakeLists.txt b/drone_simulation/CMakeLists.txt index 9c85ff5..d4b19cd 100644 --- a/drone_simulation/CMakeLists.txt +++ b/drone_simulation/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(drone_simulation_lib src/drone_physics.cpp src/port_controllers/uart_port.cpp src/port_controllers/gpio_controller.cpp + src/drone_controller.cpp ) find_package(Threads REQUIRED) diff --git a/drone_simulation/include/mission_processor.hpp b/drone_simulation/include/mission_processor.hpp index af25d16..c427a92 100644 --- a/drone_simulation/include/mission_processor.hpp +++ b/drone_simulation/include/mission_processor.hpp @@ -5,30 +5,30 @@ class MissionProcessor { public: - explicit MissionProcessor(std::unique_ptr solver); + MissionProcessor(std::unique_ptr solver, const DroneConfig& config); - auto init(const dlink::AmmoCfg& ammoCfg, float altitude, float attackSpd) -> void; + auto init(const dlink::AmmoCfg& ammoCfg, float altitude) -> void; auto process(const dlink::Telemetry& telem, const std::vector& targets, int targetCount) -> MissionResult; auto reset() -> void; auto changeSolver(std::unique_ptr ballisticSolver) -> void; private: - auto evaluateTarget( - int targetIndex, const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity, Coord& outFireCoord) -> float; - auto checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos) -> bool; - auto computeControl(const DroneTelemetry& telemetry, const Coord& firePoint) -> dlink::Control; + auto evaluateTarget(int targetIndex, + const DroneTelemetry& telemetry, + const Coord& targetPos, + const Coord& targetVelocity, + Coord& outFireCoord, + Coord& outAimCoord) -> float; + auto checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity) -> bool; std::unique_ptr ballisticSolver; AmmoParams ammo; + DroneConfig droneConfig; int prevTargetIndex = -1; float ammoFlightTime = 0.0f; float ammoHorizontalFlightDistance = 0.0f; - float acceleration = 0.0f; - float attackSpeed = 0.0f; float hitRadius = 0.0f; bool initialized = false; - - // Для обчислення швидкості цілей (чекер шле тільки позицію, не швидкість) std::vector prevTargetPositions; std::vector targetVelocities; float prevTime = -1.0f; diff --git a/drone_simulation/include/simulation.hpp b/drone_simulation/include/simulation.hpp index 93b73fc..252ef78 100644 --- a/drone_simulation/include/simulation.hpp +++ b/drone_simulation/include/simulation.hpp @@ -8,13 +8,19 @@ using json = nlohmann::json; -enum class DroneStates { Stopped, Turning, Accelerating, Moving, Decelerating }; +// order is important +// 0 STOPPED +// 1 ACCELERATING +// 2 DECELERATING +// 3 TURNING +// 4 MOVING + +enum class DroneStates { Stopped, Accelerating, Decelerating, Turning, Moving }; struct DroneCommand { DroneStates state; float angleSpeed; }; - struct Coord { float x; float y; @@ -113,15 +119,18 @@ struct Target { Coord pos; Coord velocity; }; + struct DroneTelemetry { Coord pos; float speed; float direction; float timeSecSinceStart; + DroneStates state; }; struct MissionResult { - dlink::Control control; + Coord firePoint; + Coord aimPoint; bool shouldDrop; int bestTargetIndex; }; @@ -171,4 +180,6 @@ float length(const Coord& coord); Coord normalize(const Coord& coord); -dlink::Control computeControl(const dlink::Telemetry& telem, const dlink::TargetPos& target); \ No newline at end of file +dlink::Control computeControl(const dlink::Telemetry& telem, const dlink::TargetPos& target); + +DroneTelemetry mapDlinkTelemetry(const dlink::Telemetry& telemetry); \ No newline at end of file diff --git a/drone_simulation/src/config_loaders/file_config_loader.cpp b/drone_simulation/src/config_loaders/file_config_loader.cpp index bf24284..6ad230b 100644 --- a/drone_simulation/src/config_loaders/file_config_loader.cpp +++ b/drone_simulation/src/config_loaders/file_config_loader.cpp @@ -1,28 +1,32 @@ #include "simulation.hpp" #include "config_loaders/file_config_loader.hpp" - -FileConfigLoader::FileConfigLoader( - const std::string droneConfigPath, - const std::string ammoConfigPath -): droneConfigPath(droneConfigPath), ammoConfigPath(ammoConfigPath) { +FileConfigLoader::FileConfigLoader(const std::string droneConfigPath, const std::string ammoConfigPath) + : droneConfigPath(droneConfigPath) + , ammoConfigPath(ammoConfigPath) +{ } -auto FileConfigLoader::load() -> void { - if (loaded) { - return; - } - droneConfig = readDroneConfig(droneConfigPath); +auto FileConfigLoader::load() -> void +{ + if (loaded) { + return; + } + droneConfig = readDroneConfig(droneConfigPath); + if (!ammoConfigPath.empty()) { json ammoJSON = parseJSONfile(ammoConfigPath); std::unordered_map ammoMap = getAmmoMap(ammoJSON); ammoParams = findAmmo(ammoMap, droneConfig.ammoName); - loaded = true; + } + loaded = true; }; -auto FileConfigLoader::getConfig() -> DroneConfig { - return droneConfig; +auto FileConfigLoader::getConfig() -> DroneConfig +{ + return droneConfig; }; -auto FileConfigLoader::getAmmoParams() -> AmmoParams { - return ammoParams; +auto FileConfigLoader::getAmmoParams() -> AmmoParams +{ + return ammoParams; }; \ No newline at end of file diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index 5621131..8fdaec3 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -4,15 +4,18 @@ #include #include #include "mission_processor.hpp" +#include "config_loaders/factory.hpp" #include "solvers/analytical_solver.hpp" #include "port_controllers/uart_port.hpp" #include "port_controllers/gpio_controller.hpp" +#include "drone_controller.hpp" auto main(int argc, char* argv[]) -> int { try { std::string uartDev = "/tmp/ttyA"; std::string gpioChip = "gpiochip0"; + std::string configPath = "config.json"; int startLine = 24; int dropLine = 23; @@ -30,10 +33,23 @@ auto main(int argc, char* argv[]) -> int else if (arg == "--drop-line" && i + 1 < argc) { dropLine = std::stoi(argv[++i]); } + else if (arg == "--config" && i + 1 < argc) { + configPath = argv[++i]; + } } std::cout << "UART: " << uartDev << " GPIO: " << gpioChip << " START: " << startLine << " DROP: " << dropLine << std::endl; + auto loader = createLoader(LoaderType::FILE, configPath, ""); + loader->load(); + DroneConfig droneConfig = loader->getConfig(); + auto solver = std::make_unique(droneConfig); + MissionProcessor missionProcessor(std::move(solver), droneConfig); + DroneController droneController(droneConfig); + + std::cout << "Config: attackSpeed=" << droneConfig.attackSpeed << " accelPath=" << droneConfig.accelPath + << " angularSpeed=" << droneConfig.angularSpeed << std::endl; + UartPort uart(uartDev.c_str()); GpioController gpio(gpioChip.c_str(), startLine, dropLine); @@ -51,8 +67,6 @@ auto main(int argc, char* argv[]) -> int bool dropped = false; bool mpInitialized = false; - std::unique_ptr missionProcessor; - dlink::Parser parser; uint8_t buffer[256]; @@ -74,26 +88,17 @@ auto main(int argc, char* argv[]) -> int std::memcpy(&telemetry, payload, sizeof(telemetry)); std::cout << "t=" << telemetry.t_ms << " pos=(" << telemetry.x << "," << telemetry.y << ")" << " speed=" << telemetry.speed - << " dir=" << telemetry.dir << std::endl; + << " dir=" << telemetry.dir << " z=" << telemetry.z << " state=" << (int)telemetry.state << std::endl; - // Створити солвер і MP після першої телеметрії — тепер знаємо altitude і speed if (ammoReceived && !mpInitialized) { - float speed = telemetry.speed > 1.0f ? telemetry.speed : 30.0f; - - DroneConfig solverConfig{}; - solverConfig.altitude = telemetry.z; - solverConfig.attackSpeed = speed; - - auto solver = std::make_unique(solverConfig); - missionProcessor = std::make_unique(std::move(solver)); - missionProcessor->init(ammo, telemetry.z, speed); + std::cout << "MP init with altitude=" << telemetry.z << std::endl; + missionProcessor.init(ammo, telemetry.z); mpInitialized = true; } - // Отримати рішення від MP dlink::Control control{0.0f, 0.0f}; if (mpInitialized) { - MissionResult result = missionProcessor->process(telemetry, targetPositions, targetCount); + MissionResult result = missionProcessor.process(telemetry, targetPositions, targetCount); if (result.shouldDrop && !dropped) { std::cout << "DROP!" << std::endl; @@ -101,7 +106,8 @@ auto main(int argc, char* argv[]) -> int dropped = true; } - control = result.control; + DroneTelemetry droneTelemetry = mapDlinkTelemetry(telemetry); + control = droneController.computeControl(droneTelemetry, result.aimPoint); } uint8_t out[64]; diff --git a/drone_simulation/src/mission_processor.cpp b/drone_simulation/src/mission_processor.cpp index 50ff3bc..1abf4bc 100644 --- a/drone_simulation/src/mission_processor.cpp +++ b/drone_simulation/src/mission_processor.cpp @@ -4,182 +4,159 @@ #include #include "mission_processor.hpp" -MissionProcessor::MissionProcessor(std::unique_ptr solver) +MissionProcessor::MissionProcessor(std::unique_ptr solver, const DroneConfig& config) : ballisticSolver(std::move(solver)) + , droneConfig(config) { } -auto MissionProcessor::init(const dlink::AmmoCfg& ammoCfg, float altitude, float attackSpd) -> void +auto MissionProcessor::init(const dlink::AmmoCfg& ammoCfg, float altitude) -> void { this->ammo = {.name = std::string(ammoCfg.name), .mass = ammoCfg.mass, .drag = ammoCfg.drag, .lift = ammoCfg.lift}; this->hitRadius = ammoCfg.hitRadius; - this->attackSpeed = attackSpd; - float accelPath = 50.0f; - this->acceleration = this->attackSpeed * this->attackSpeed / (2.0f * accelPath); - - DroneConfig tempConfig{}; - tempConfig.altitude = altitude; - tempConfig.attackSpeed = this->attackSpeed; - - this->ammoFlightTime = getAmmoFlightTime(tempConfig, this->ammo); - this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->attackSpeed, this->ammoFlightTime, this->ammo); + this->droneConfig.altitude = altitude; + this->ammoFlightTime = getAmmoFlightTime(this->droneConfig, this->ammo); + this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->droneConfig.attackSpeed, this->ammoFlightTime, this->ammo); this->initialized = true; - std::cout << "MP init: flightTime=" << ammoFlightTime << " horizDist=" << ammoHorizontalFlightDistance << std::endl; + std::cout << "MP init: flightTime=" << this->ammoFlightTime << " horizDist=" << this->ammoHorizontalFlightDistance << std::endl; } -auto MissionProcessor::process(const dlink::Telemetry& telem, +auto MissionProcessor::process(const dlink::Telemetry& telemetry, const std::vector& targets, int targetCount) -> MissionResult { if (!this->initialized || targetCount <= 0) { - return {dlink::Control{0.0f, 0.0f}, false, -1}; + return {Coord{0, 0}, Coord{0, 0}, false, -1}; } - DroneTelemetry droneTelem = { - .pos = {telem.x, telem.y}, .speed = telem.speed, .direction = telem.dir, .timeSecSinceStart = telem.t_ms / 1000.0f}; + DroneTelemetry droneTelemetry = { + .pos = {telemetry.x, telemetry.y}, .speed = telemetry.speed, .direction = telemetry.dir, .timeSecSinceStart = telemetry.t_ms / 1000.0f}; + float currentTime = telemetry.t_ms / 1000.0f; - // Обчислити швидкості цілей з різниці позицій між тактами - float currentTime = telem.t_ms / 1000.0f; - - if (prevTime >= 0.0f && !prevTargetPositions.empty()) { - float dt = currentTime - prevTime; + if (this->prevTime >= 0.0f && !this->prevTargetPositions.empty()) { + float dt = currentTime - this->prevTime; if (dt > 1e-6f) { - targetVelocities.resize(targetCount, {0.0f, 0.0f}); - for (int i = 0; i < targetCount && i < (int)targets.size() && i < (int)prevTargetPositions.size(); ++i) { + this->targetVelocities.resize(targetCount, {0.0f, 0.0f}); + for (int i = 0; i < targetCount && i < (int)targets.size() && i < (int)this->prevTargetPositions.size(); ++i) { Coord current = {targets[i].x, targets[i].y}; - targetVelocities[i] = (current - prevTargetPositions[i]) / dt; + this->targetVelocities[i] = (current - this->prevTargetPositions[i]) / dt; } } } else { - targetVelocities.resize(targetCount, {0.0f, 0.0f}); + this->targetVelocities.resize(targetCount, {0.0f, 0.0f}); } - // Зберегти поточні позиції для наступного такту - prevTargetPositions.resize(targetCount); + this->prevTargetPositions.resize(targetCount); for (int i = 0; i < targetCount && i < (int)targets.size(); ++i) { - prevTargetPositions[i] = {targets[i].x, targets[i].y}; + this->prevTargetPositions[i] = {targets[i].x, targets[i].y}; } - prevTime = currentTime; + this->prevTime = currentTime; - // Знайти найкращу ціль int bestTargetIndex = -1; float bestTotalTime = INFINITY; Coord bestFireCoord = {0, 0}; + Coord bestAimCoord = {0, 0}; for (int i = 0; i < targetCount && i < (int)targets.size(); ++i) { Coord targetPos = {targets[i].x, targets[i].y}; - Coord velocity = (i < (int)targetVelocities.size()) ? targetVelocities[i] : Coord{0, 0}; + Coord velocity = (i < (int)this->targetVelocities.size()) ? this->targetVelocities[i] : Coord{0, 0}; Coord fireCoord; - float totalTime = evaluateTarget(i, droneTelem, targetPos, velocity, fireCoord); + Coord aimCoord; + float totalTime = this->evaluateTarget(i, droneTelemetry, targetPos, velocity, fireCoord, aimCoord); if (totalTime < bestTotalTime) { bestTotalTime = totalTime; bestTargetIndex = i; bestFireCoord = fireCoord; + bestAimCoord = aimCoord; } } if (bestTargetIndex < 0) { - return {dlink::Control{0.0f, 0.0f}, false, -1}; + return {Coord{0, 0}, Coord{0, 0}, false, -1}; } Coord bestTargetPos = {targets[bestTargetIndex].x, targets[bestTargetIndex].y}; - bool shouldDrop = checkDrop(droneTelem, bestTargetPos); - - dlink::Control control = computeControl(droneTelem, bestFireCoord); + Coord bestVelocity = (bestTargetIndex < (int)this->targetVelocities.size()) ? this->targetVelocities[bestTargetIndex] : Coord{0, 0}; + bool shouldDrop = this->checkDrop(droneTelemetry, bestTargetPos, bestVelocity); this->prevTargetIndex = bestTargetIndex; - return {control, shouldDrop, bestTargetIndex}; + return {bestFireCoord, bestAimCoord, shouldDrop, bestTargetIndex}; } -auto MissionProcessor::evaluateTarget( - int targetIndex, const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity, Coord& outFireCoord) -> float +auto MissionProcessor::evaluateTarget(int targetIndex, + const DroneTelemetry& telemetry, + const Coord& targetPos, + const Coord& targetVelocity, + Coord& outFireCoord, + Coord& outAimCoord) -> float { - // Прохід 1: грубий — fire point для поточної позиції цілі + float currentHorizDist = getAmmoHorizontalFlightDistance(telemetry.speed, this->ammoFlightTime, this->ammo); + float distanceToTarget = getDistanceToTarget(targetPos, telemetry.pos); if (distanceToTarget < 1e-3f) { return INFINITY; } - float ratio = getRatio(distanceToTarget, this->ammoHorizontalFlightDistance); + float ratio = getRatio(distanceToTarget, currentHorizDist); Coord fireCoord = getFireCoords(targetPos, telemetry.pos, ratio); float distanceToFire = getDistanceToTarget(fireCoord, telemetry.pos); - float droneToFireTime = distanceToFire / this->attackSpeed; + float droneToFireTime = distanceToFire / std::max(telemetry.speed, 1.0f); float totalTimeRough = droneToFireTime + this->ammoFlightTime; - // Прохід 2: точний — передбачити де буде ціль через totalTimeRough Coord predictedTarget = targetPos + targetVelocity * totalTimeRough; - + outAimCoord = predictedTarget; float newDistanceToTarget = getDistanceToTarget(predictedTarget, telemetry.pos); if (newDistanceToTarget < 1e-3f) { return INFINITY; } - float newRatio = getRatio(newDistanceToTarget, this->ammoHorizontalFlightDistance); + float newRatio = getRatio(newDistanceToTarget, currentHorizDist); outFireCoord = getFireCoords(predictedTarget, telemetry.pos, newRatio); float newDistanceToFire = getDistanceToTarget(outFireCoord, telemetry.pos); - float newDroneToFireTime = newDistanceToFire / this->attackSpeed; + float newDroneToFireTime = newDistanceToFire / std::max(telemetry.speed, 1.0f); float totalTime = newDroneToFireTime + this->ammoFlightTime; + float acceleration = this->droneConfig.attackSpeed * this->droneConfig.attackSpeed / (2.0f * this->droneConfig.accelPath); - // Штраф за маневр якщо fire point за дроном if (newRatio < 0) { - totalTime += getManeuveringTime(newDistanceToTarget, this->ammoHorizontalFlightDistance, this->attackSpeed, this->acceleration, 1.0f); + totalTime += getManeuveringTime( + newDistanceToTarget, this->ammoHorizontalFlightDistance, this->droneConfig.attackSpeed, acceleration, this->droneConfig.angularSpeed); } - // Штраф за зміну цілі if (this->prevTargetIndex != targetIndex && this->prevTargetIndex != -1) { - totalTime += telemetry.speed / this->acceleration; + totalTime += telemetry.speed / acceleration; } return totalTime; } -auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos) -> bool -{ - Coord bombLand = getBombLandCoord(telemetry, ammoHorizontalFlightDistance); - float miss = getDistanceToTarget(bombLand, targetPos); - - float dx = targetPos.x - telemetry.pos.x; - float dy = targetPos.y - telemetry.pos.y; - float desiredDir = std::atan2(dy, dx); - float deltaAngle = normalizeAngle(desiredDir - telemetry.direction); - - return miss <= hitRadius && telemetry.speed > 5.0f && std::fabs(deltaAngle) < 0.3f; -} - -auto MissionProcessor::computeControl(const DroneTelemetry& telemetry, const Coord& firePoint) -> dlink::Control +auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity) -> bool { - float dx = firePoint.x - telemetry.pos.x; - float dy = firePoint.y - telemetry.pos.y; - float desiredDir = std::atan2(dy, dx); - float deltaAngle = normalizeAngle(desiredDir - telemetry.direction); + float currentHorizDist = getAmmoHorizontalFlightDistance(telemetry.speed, this->ammoFlightTime, this->ammo); + Coord bombLandCoord = getBombLandCoord(telemetry, currentHorizDist); - float Kp = 2.0f; - float turnRate = std::clamp(Kp * deltaAngle, -1.0f, 1.0f); + // ціль рухається, поки бомба летить ammoFlightTime — порівнюємо з тим, де ціль БУДЕ + Coord predictedTarget = targetPos + targetVelocity * this->ammoFlightTime; + float bombMissDistance = getDistanceToTarget(bombLandCoord, predictedTarget); - float accel; - if (std::fabs(deltaAngle) > 0.3f) { - accel = 0.2f; - } - else { - accel = 1.0f; - } + std::cout << " CD: miss=" << bombMissDistance << " predTgt=(" << predictedTarget.x << "," << predictedTarget.y << ")" << " bomb=(" + << bombLandCoord.x << "," << bombLandCoord.y << ")" << " hd=" << currentHorizDist << std::endl; - return dlink::Control{accel, turnRate}; + return bombMissDistance <= this->hitRadius && telemetry.speed >= this->droneConfig.attackSpeed; } auto MissionProcessor::reset() -> void { - prevTargetIndex = -1; - prevTime = -1.0f; - prevTargetPositions.clear(); - targetVelocities.clear(); - initialized = false; + this->prevTargetIndex = -1; + this->prevTime = -1.0f; + this->prevTargetPositions.clear(); + this->targetVelocities.clear(); + this->initialized = false; } auto MissionProcessor::changeSolver(std::unique_ptr ballisticSolver) -> void diff --git a/drone_simulation/src/simulation.cpp b/drone_simulation/src/simulation.cpp index f82e676..5f2904c 100644 --- a/drone_simulation/src/simulation.cpp +++ b/drone_simulation/src/simulation.cpp @@ -304,4 +304,13 @@ dlink::Control computeControl(const dlink::Telemetry& telemetry, const dlink::Ta } return dlink::Control{accel, turnRate}; +} + +auto mapDlinkTelemetry(const dlink::Telemetry& telemetry) -> DroneTelemetry +{ + return {.pos = {telemetry.x, telemetry.y}, + .speed = telemetry.speed, + .direction = telemetry.dir, + .timeSecSinceStart = telemetry.t_ms / 1000.0f, + .state = static_cast(telemetry.state)}; } \ No newline at end of file From e713f03a6e9bb8366c9ef9f9be00add437c9896a Mon Sep 17 00:00:00 2001 From: blinker Date: Mon, 29 Jun 2026 10:13:25 +0000 Subject: [PATCH 06/10] [FEATURE] hw11-uart: added drone_controller to git --- drone_simulation/include/drone_controller.hpp | 15 ++++++ drone_simulation/src/drone_controller.cpp | 46 +++++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 drone_simulation/include/drone_controller.hpp create mode 100644 drone_simulation/src/drone_controller.cpp diff --git a/drone_simulation/include/drone_controller.hpp b/drone_simulation/include/drone_controller.hpp new file mode 100644 index 0000000..b091bd4 --- /dev/null +++ b/drone_simulation/include/drone_controller.hpp @@ -0,0 +1,15 @@ +#pragma once +#include "simulation.hpp" +#include "drone_link.hpp" + +class DroneController { +public: + DroneController(const DroneConfig& config); + auto computeControl(const DroneTelemetry& telemetry, const Coord& aimPoint) -> dlink::Control; + +private: + DroneConfig config; + float prevDesiredDir = 0.0f; + float prevTime = -1.0f; + bool hasPrev = false; +}; \ No newline at end of file diff --git a/drone_simulation/src/drone_controller.cpp b/drone_simulation/src/drone_controller.cpp new file mode 100644 index 0000000..aba0607 --- /dev/null +++ b/drone_simulation/src/drone_controller.cpp @@ -0,0 +1,46 @@ +#include +#include "simulation.hpp" +#include "drone_controller.hpp" + +DroneController::DroneController(const DroneConfig& config) + : config(config) +{ +} + +auto DroneController::computeControl(const DroneTelemetry& telemetry, const Coord& aimPoint) -> dlink::Control +{ + float dx = aimPoint.x - telemetry.pos.x; + float dy = aimPoint.y - telemetry.pos.y; + float desiredDir = std::atan2(dy, dx); + float deltaAngle = normalizeAngle(desiredDir - telemetry.direction); + + // швидкість обертання лінії візування (LOS rate) — щоб не відставати від рухомої цілі + float losRate = 0.0f; + if (this->hasPrev) { + float dt = telemetry.timeSecSinceStart - this->prevTime; + if (dt > 1e-3f) { + losRate = normalizeAngle(desiredDir - this->prevDesiredDir) / dt; + } + } + this->prevDesiredDir = desiredDir; + this->prevTime = telemetry.timeSecSinceStart; + this->hasPrev = true; + + // PD-структура з feed-forward похідної орієнтира + const float Kp = 3.0f; + const float Kd = 1.5f; + float turnRate = std::clamp(Kp * deltaAngle + Kd * losRate, -1.0f, 1.0f); + + float accel; + if (std::fabs(deltaAngle) > this->config.turnThreshold) { + accel = 0.0f; + } + else if (telemetry.speed >= this->config.attackSpeed) { + float speedError = telemetry.speed - this->config.attackSpeed; + accel = std::clamp(-2.0f * speedError, -1.0f, 0.0f); + } + else { + accel = 1.0f; + } + return dlink::Control{accel, turnRate}; +} From 3544a5652f7e387a88a2631431642c8c49ba9d3b Mon Sep 17 00:00:00 2001 From: blinker Date: Wed, 1 Jul 2026 14:42:53 +0000 Subject: [PATCH 07/10] [FIX]: use forward declaration in factory.hpp --- drone_simulation/include/config_loaders/factory.hpp | 11 ++++++----- drone_simulation/include/drone_controller.hpp | 1 - drone_simulation/src/drone_controller.cpp | 3 ++- drone_simulation/src/main.cpp | 1 + 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/drone_simulation/include/config_loaders/factory.hpp b/drone_simulation/include/config_loaders/factory.hpp index 6fb38c8..a5a1d38 100644 --- a/drone_simulation/include/config_loaders/factory.hpp +++ b/drone_simulation/include/config_loaders/factory.hpp @@ -1,10 +1,11 @@ #pragma once -#include #include -#include "simulation.hpp" -#include "interfaces/ballistic_solver.hpp" -#include "interfaces/target_provider.hpp" -#include "interfaces/config_loader.hpp" +#include + +struct DroneConfig; +class IBallisticSolver; +class ITargetProvider; +class IConfigLoader; enum class SolverType { ANALYTICAL, TABLE }; enum class ProviderType { JSON, THREAD_SAFE }; diff --git a/drone_simulation/include/drone_controller.hpp b/drone_simulation/include/drone_controller.hpp index b091bd4..17a393a 100644 --- a/drone_simulation/include/drone_controller.hpp +++ b/drone_simulation/include/drone_controller.hpp @@ -1,6 +1,5 @@ #pragma once #include "simulation.hpp" -#include "drone_link.hpp" class DroneController { public: diff --git a/drone_simulation/src/drone_controller.cpp b/drone_simulation/src/drone_controller.cpp index aba0607..ae8c0bf 100644 --- a/drone_simulation/src/drone_controller.cpp +++ b/drone_simulation/src/drone_controller.cpp @@ -42,5 +42,6 @@ auto DroneController::computeControl(const DroneTelemetry& telemetry, const Coor else { accel = 1.0f; } + return dlink::Control{accel, turnRate}; -} +} \ No newline at end of file diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index 8fdaec3..d57fa47 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -9,6 +9,7 @@ #include "port_controllers/uart_port.hpp" #include "port_controllers/gpio_controller.hpp" #include "drone_controller.hpp" +#include "interfaces/config_loader.hpp" auto main(int argc, char* argv[]) -> int { From 194bb5b85399aa8de464e8f6faf2f6d507073a70 Mon Sep 17 00:00:00 2001 From: blinker Date: Wed, 1 Jul 2026 15:06:39 +0000 Subject: [PATCH 08/10] [FIX]: added log.hpp with log macros --- drone_simulation/include/log.hpp | 17 ++++++++++++++++ .../port_controllers/gpio_controller.hpp | 12 +++-------- .../include/port_controllers/uart_port.hpp | 7 +------ drone_simulation/src/main.cpp | 20 +++++++++---------- drone_simulation/src/mission_processor.cpp | 7 +++---- drone_simulation/src/simulation.cpp | 17 ---------------- 6 files changed, 34 insertions(+), 46 deletions(-) create mode 100644 drone_simulation/include/log.hpp diff --git a/drone_simulation/include/log.hpp b/drone_simulation/include/log.hpp new file mode 100644 index 0000000..16ed4f2 --- /dev/null +++ b/drone_simulation/include/log.hpp @@ -0,0 +1,17 @@ +#pragma once +#include + +#define ENABLE_LOG 1 +#define ENABLE_DEBUG 0 + +#if ENABLE_LOG +#define LOG(msg) std::cout << "[LOG] " << msg << std::endl +#else +#define LOG(msg) +#endif + +#if ENABLE_DEBUG +#define DEBUG(msg) std::cout << "[DEBUG] " << msg << std::endl +#else +#define DEBUG(msg) +#endif \ No newline at end of file diff --git a/drone_simulation/include/port_controllers/gpio_controller.hpp b/drone_simulation/include/port_controllers/gpio_controller.hpp index 41a688e..1222015 100644 --- a/drone_simulation/include/port_controllers/gpio_controller.hpp +++ b/drone_simulation/include/port_controllers/gpio_controller.hpp @@ -3,20 +3,14 @@ class GpioController { public: - // chipName — ім'я GPIO контролера: "gpiochip0" (Raspberry Pi) або "gpiochip1" (симуляція) - // startLine — номер піна для сигналу START (24 за замовчуванням) - // dropLine — номер піна для сигналу DROP (23 за замовчуванням) GpioController(const char* chipName, int startLine, int dropLine); ~GpioController(); - // Підняти START у 1 — сигнал чекеру "я готовий, починай симуляцію" auto signalStart() -> void; - - // Імпульс DROP: 1 → чекати 80мс → 0. Чекер ловить фронт і рахує балістику. auto signalDrop() -> void; private: - gpiod_chip* chip; // "чіп" — контролер, що містить набір GPIO ліній - gpiod_line* startLine; // конкретна лінія (пін) для START - gpiod_line* dropLine; // конкретна лінія (пін) для DROP + gpiod_chip* chip; + gpiod_line* startLine; + gpiod_line* dropLine; }; \ No newline at end of file diff --git a/drone_simulation/include/port_controllers/uart_port.hpp b/drone_simulation/include/port_controllers/uart_port.hpp index a9426be..3d3fe02 100644 --- a/drone_simulation/include/port_controllers/uart_port.hpp +++ b/drone_simulation/include/port_controllers/uart_port.hpp @@ -4,17 +4,12 @@ class UartPort { public: - // device — шлях до порту: "/tmp/ttyA" (симуляція) або "/dev/ttyAMA1" (Raspberry Pi) explicit UartPort(const char* device); ~UartPort(); - // Прочитати доступні байти з порту. Повертає кількість прочитаних байтів, - // або -1 якщо даних поки нема (бо порт відкритий з O_NONBLOCK). auto readBytes(uint8_t* buffer, size_t maxByteCount) -> int; - - // Записати байти в порт — відправити дані на інший кінець. auto writeBytes(const uint8_t* buffer, size_t byteCount) -> void; private: - int fd; // file descriptor — "номер" відкритого файлу в Linux + int fd; }; \ No newline at end of file diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index d57fa47..9f9b018 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -10,6 +10,7 @@ #include "port_controllers/gpio_controller.hpp" #include "drone_controller.hpp" #include "interfaces/config_loader.hpp" +#include "log.hpp" auto main(int argc, char* argv[]) -> int { @@ -39,7 +40,7 @@ auto main(int argc, char* argv[]) -> int } } - std::cout << "UART: " << uartDev << " GPIO: " << gpioChip << " START: " << startLine << " DROP: " << dropLine << std::endl; + DEBUG("UART: " << uartDev << " GPIO: " << gpioChip << " START: " << startLine << " DROP: " << dropLine); auto loader = createLoader(LoaderType::FILE, configPath, ""); loader->load(); @@ -48,14 +49,14 @@ auto main(int argc, char* argv[]) -> int MissionProcessor missionProcessor(std::move(solver), droneConfig); DroneController droneController(droneConfig); - std::cout << "Config: attackSpeed=" << droneConfig.attackSpeed << " accelPath=" << droneConfig.accelPath - << " angularSpeed=" << droneConfig.angularSpeed << std::endl; + DEBUG("Config: attackSpeed=" << droneConfig.attackSpeed << " accelPath=" << droneConfig.accelPath + << " angularSpeed=" << droneConfig.angularSpeed); UartPort uart(uartDev.c_str()); GpioController gpio(gpioChip.c_str(), startLine, dropLine); gpio.signalStart(); - std::cout << "START signal sent, waiting for data..." << std::endl; + LOG("START signal sent, waiting for data..."); dlink::AmmoCfg ammo{}; bool ammoReceived = false; @@ -88,11 +89,10 @@ auto main(int argc, char* argv[]) -> int case dlink::PKT_TELEMETRY: { std::memcpy(&telemetry, payload, sizeof(telemetry)); - std::cout << "t=" << telemetry.t_ms << " pos=(" << telemetry.x << "," << telemetry.y << ")" << " speed=" << telemetry.speed - << " dir=" << telemetry.dir << " z=" << telemetry.z << " state=" << (int)telemetry.state << std::endl; + DEBUG("t=" << telemetry.t_ms << " pos=(" << telemetry.x << "," << telemetry.y << ")" << " speed=" << telemetry.speed + << " dir=" << telemetry.dir << " z=" << telemetry.z << " state=" << (int)telemetry.state); if (ammoReceived && !mpInitialized) { - std::cout << "MP init with altitude=" << telemetry.z << std::endl; missionProcessor.init(ammo, telemetry.z); mpInitialized = true; } @@ -102,7 +102,7 @@ auto main(int argc, char* argv[]) -> int MissionResult result = missionProcessor.process(telemetry, targetPositions, targetCount); if (result.shouldDrop && !dropped) { - std::cout << "DROP!" << std::endl; + LOG("DROP!"); gpio.signalDrop(); dropped = true; } @@ -122,8 +122,8 @@ auto main(int argc, char* argv[]) -> int ammoReceived = true; targetCount = ammo.nTargets; targetPositions.resize(targetCount); - std::cout << "AMMO: " << ammo.name << " mass=" << ammo.mass << " drag=" << ammo.drag << " lift=" << ammo.lift - << " hitRadius=" << ammo.hitRadius << " targets=" << (int)ammo.nTargets << std::endl; + LOG("AMMO: " << ammo.name << " mass=" << ammo.mass << " drag=" << ammo.drag << " lift=" << ammo.lift + << " hitRadius=" << ammo.hitRadius << " targets=" << (int)ammo.nTargets); break; } diff --git a/drone_simulation/src/mission_processor.cpp b/drone_simulation/src/mission_processor.cpp index 1abf4bc..1b5cd28 100644 --- a/drone_simulation/src/mission_processor.cpp +++ b/drone_simulation/src/mission_processor.cpp @@ -3,6 +3,7 @@ #include #include #include "mission_processor.hpp" +#include "log.hpp" MissionProcessor::MissionProcessor(std::unique_ptr solver, const DroneConfig& config) : ballisticSolver(std::move(solver)) @@ -20,8 +21,6 @@ auto MissionProcessor::init(const dlink::AmmoCfg& ammoCfg, float altitude) -> vo this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->droneConfig.attackSpeed, this->ammoFlightTime, this->ammo); this->initialized = true; - - std::cout << "MP init: flightTime=" << this->ammoFlightTime << " horizDist=" << this->ammoHorizontalFlightDistance << std::endl; } auto MissionProcessor::process(const dlink::Telemetry& telemetry, @@ -144,8 +143,8 @@ auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& t Coord predictedTarget = targetPos + targetVelocity * this->ammoFlightTime; float bombMissDistance = getDistanceToTarget(bombLandCoord, predictedTarget); - std::cout << " CD: miss=" << bombMissDistance << " predTgt=(" << predictedTarget.x << "," << predictedTarget.y << ")" << " bomb=(" - << bombLandCoord.x << "," << bombLandCoord.y << ")" << " hd=" << currentHorizDist << std::endl; + DEBUG(" CD: miss=" << bombMissDistance << " predTgt=(" << predictedTarget.x << "," << predictedTarget.y << ")" << " bomb=(" + << bombLandCoord.x << "," << bombLandCoord.y << ")" << " hd=" << currentHorizDist); return bombMissDistance <= this->hitRadius && telemetry.speed >= this->droneConfig.attackSpeed; } diff --git a/drone_simulation/src/simulation.cpp b/drone_simulation/src/simulation.cpp index 5f2904c..0acf14e 100644 --- a/drone_simulation/src/simulation.cpp +++ b/drone_simulation/src/simulation.cpp @@ -7,21 +7,6 @@ using json = nlohmann::json; -#define ENABLE_LOG 1 -#define ENABLE_DEBUG 0 - -#if ENABLE_LOG -#define LOG(msg) std::cout << "[LOG] " << msg << std::endl -#else -#define LOG(msg) -#endif - -#if ENABLE_DEBUG -#define DEBUG(msg) std::cout << "[DEBUG] " << msg << std::endl -#else -#define DEBUG(msg) -#endif - float getDistanceToTarget(const Coord& target, const Coord& posiiton) { return sqrt(pow(target.x - posiiton.x, 2) + pow(target.y - posiiton.y, 2)); @@ -201,8 +186,6 @@ DroneConfig readDroneConfig(const std::string& path) droneConfig.ammoName = configJSON["ammo"].get(); - DEBUG("ammo config: " << droneConfig.ammoName); - return droneConfig; } From 561762e00ca1543528d16be3696b6e633404f4c5 Mon Sep 17 00:00:00 2001 From: blinker Date: Wed, 1 Jul 2026 15:11:31 +0000 Subject: [PATCH 09/10] [FEATURE] hw11-uart: remove redundant function --- drone_simulation/include/simulation.hpp | 2 -- drone_simulation/src/mission_processor.cpp | 1 - drone_simulation/src/simulation.cpp | 26 ---------------------- 3 files changed, 29 deletions(-) diff --git a/drone_simulation/include/simulation.hpp b/drone_simulation/include/simulation.hpp index 252ef78..c97c418 100644 --- a/drone_simulation/include/simulation.hpp +++ b/drone_simulation/include/simulation.hpp @@ -180,6 +180,4 @@ float length(const Coord& coord); Coord normalize(const Coord& coord); -dlink::Control computeControl(const dlink::Telemetry& telem, const dlink::TargetPos& target); - DroneTelemetry mapDlinkTelemetry(const dlink::Telemetry& telemetry); \ No newline at end of file diff --git a/drone_simulation/src/mission_processor.cpp b/drone_simulation/src/mission_processor.cpp index 1b5cd28..c7cd60e 100644 --- a/drone_simulation/src/mission_processor.cpp +++ b/drone_simulation/src/mission_processor.cpp @@ -139,7 +139,6 @@ auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& t float currentHorizDist = getAmmoHorizontalFlightDistance(telemetry.speed, this->ammoFlightTime, this->ammo); Coord bombLandCoord = getBombLandCoord(telemetry, currentHorizDist); - // ціль рухається, поки бомба летить ammoFlightTime — порівнюємо з тим, де ціль БУДЕ Coord predictedTarget = targetPos + targetVelocity * this->ammoFlightTime; float bombMissDistance = getDistanceToTarget(bombLandCoord, predictedTarget); diff --git a/drone_simulation/src/simulation.cpp b/drone_simulation/src/simulation.cpp index 0acf14e..6711990 100644 --- a/drone_simulation/src/simulation.cpp +++ b/drone_simulation/src/simulation.cpp @@ -263,32 +263,6 @@ Coord normalize(const Coord& coord) return coord / length(coord); } -dlink::Control computeControl(const dlink::Telemetry& telemetry, const dlink::TargetPos& target) -{ - float dx = target.x - telemetry.x; - float dy = target.y - telemetry.y; - - float desiredDir = std::atan2(dy, dx); - - float deltaAngle = normalizeAngle(desiredDir - telemetry.dir); - - // turnRate — P-регулятор - // TODO: check if it's needed - float Kp = 2.0f; - float turnRate = std::clamp(Kp * deltaAngle, -1.0f, 1.0f); - - // accel — газ або гальмо залежно від курсу - float accel; - if (std::fabs(deltaAngle) > 0.3f) { - accel = 0.2f; // не розганятись поки не повернулись - } - else { - accel = 1.0f; // курс правильний — повний газ - } - - return dlink::Control{accel, turnRate}; -} - auto mapDlinkTelemetry(const dlink::Telemetry& telemetry) -> DroneTelemetry { return {.pos = {telemetry.x, telemetry.y}, From 3436d492beb4c398e88b841cccf903da15c4815c Mon Sep 17 00:00:00 2001 From: blinker Date: Fri, 14 Aug 2026 18:03:58 +0000 Subject: [PATCH 10/10] [FEATURE] hw11-uart2: add include --- .../include/providers/thread_safe_target_provider.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/drone_simulation/include/providers/thread_safe_target_provider.hpp b/drone_simulation/include/providers/thread_safe_target_provider.hpp index 7d18792..d3d7437 100644 --- a/drone_simulation/include/providers/thread_safe_target_provider.hpp +++ b/drone_simulation/include/providers/thread_safe_target_provider.hpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "interfaces/target_provider.hpp" class ThreadSafeTargetProvider : public ITargetProvider {