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..d4b19cd 100644 --- a/drone_simulation/CMakeLists.txt +++ b/drone_simulation/CMakeLists.txt @@ -16,16 +16,22 @@ 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 + src/drone_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}) -target_include_directories(drone_simulation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_compile_options(drone_simulation_lib PRIVATE -Wall -Wextra -pedantic) +find_package(PkgConfig REQUIRED) +pkg_check_modules(GPIOD REQUIRED libgpiod) + +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) +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/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 new file mode 100644 index 0000000..17a393a --- /dev/null +++ b/drone_simulation/include/drone_controller.hpp @@ -0,0 +1,14 @@ +#pragma once +#include "simulation.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/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 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/mission_processor.hpp b/drone_simulation/include/mission_processor.hpp index bc79998..c427a92 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); + MissionProcessor(std::unique_ptr solver, const DroneConfig& config); - auto init(std::unique_ptr configLoader) -> void; - auto getCurrentStep() -> int; - auto step() -> SimulationStep; + 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; - 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, + Coord& outAimCoord) -> float; + auto checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity) -> bool; - const int MAX_STEPS = 10000; - std::unique_ptr targets; std::unique_ptr ballisticSolver; - DroneConfig droneConfig; AmmoParams ammo; - int currentStep = 1; + DroneConfig droneConfig; 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 hitRadius = 0.0f; + bool initialized = false; + std::vector prevTargetPositions; + std::vector targetVelocities; + float prevTime = -1.0f; }; 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..1222015 --- /dev/null +++ b/drone_simulation/include/port_controllers/gpio_controller.hpp @@ -0,0 +1,16 @@ +#pragma once +#include // libgpiod — Linux API для роботи з GPIO + +class GpioController { +public: + GpioController(const char* chipName, int startLine, int dropLine); + ~GpioController(); + + auto signalStart() -> void; + auto signalDrop() -> void; + +private: + 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 new file mode 100644 index 0000000..3d3fe02 --- /dev/null +++ b/drone_simulation/include/port_controllers/uart_port.hpp @@ -0,0 +1,15 @@ +#pragma once +#include +#include + +class UartPort { +public: + explicit UartPort(const char* device); + ~UartPort(); + + auto readBytes(uint8_t* buffer, size_t maxByteCount) -> int; + auto writeBytes(const uint8_t* buffer, size_t byteCount) -> void; + +private: + int fd; +}; \ No newline at end of file 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 { diff --git a/drone_simulation/include/simulation.hpp b/drone_simulation/include/simulation.hpp index 3a3ac75..c97c418 100644 --- a/drone_simulation/include/simulation.hpp +++ b/drone_simulation/include/simulation.hpp @@ -4,16 +4,23 @@ #include #include #include "json.hpp" +#include "drone_link.hpp" 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; @@ -112,11 +119,20 @@ struct Target { Coord pos; Coord velocity; }; + struct DroneTelemetry { Coord pos; float speed; float direction; float timeSecSinceStart; + DroneStates state; +}; + +struct MissionResult { + Coord firePoint; + Coord aimPoint; + bool shouldDrop; + int bestTargetIndex; }; float getDistanceToTarget(const Coord& target, const Coord& posiiton); @@ -162,4 +178,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); + +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/drone_controller.cpp b/drone_simulation/src/drone_controller.cpp new file mode 100644 index 0000000..ae8c0bf --- /dev/null +++ b/drone_simulation/src/drone_controller.cpp @@ -0,0 +1,47 @@ +#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}; +} \ No newline at end of file diff --git a/drone_simulation/src/main.cpp b/drone_simulation/src/main.cpp index 04c4dcd..9f9b018 100644 --- a/drone_simulation/src/main.cpp +++ b/drone_simulation/src/main.cpp @@ -1,86 +1,149 @@ #include #include #include -#include "json.hpp" -#include "simulation.hpp" +#include +#include #include "mission_processor.hpp" #include "config_loaders/factory.hpp" -#include "providers/thread_safe_target_provider.hpp" - -using json = nlohmann::json; - -#define ENABLE_LOG 0 -#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 +#include "solvers/analytical_solver.hpp" +#include "port_controllers/uart_port.hpp" +#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 { 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"; + std::string uartDev = "/tmp/ttyA"; + std::string gpioChip = "gpiochip0"; + std::string configPath = "config.json"; + 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]); + } + else if (arg == "--config" && i + 1 < argc) { + configPath = argv[++i]; + } + } - auto loader = createLoader(LoaderType::FILE, configPath, ammoPath); + DEBUG("UART: " << uartDev << " GPIO: " << gpioChip << " START: " << startLine << " DROP: " << dropLine); + auto loader = createLoader(LoaderType::FILE, configPath, ""); 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); - - if (ballisticSolver == nullptr) { - LOG("Error: " << "Unknown ballistic solver"); - - return 1; + auto solver = std::make_unique(droneConfig); + MissionProcessor missionProcessor(std::move(solver), droneConfig); + DroneController droneController(droneConfig); + + 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(); + LOG("START signal sent, waiting for data..."); + + dlink::AmmoCfg ammo{}; + bool ammoReceived = false; + + std::vector targetPositions; + int targetCount = 0; + + dlink::Telemetry telemetry{}; + + bool dropped = false; + bool mpInitialized = 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++) { + if (!parser.feed(buffer[i], type, payload, len)) { + continue; + } + + switch (type) { + case dlink::PKT_TELEMETRY: { + std::memcpy(&telemetry, payload, sizeof(telemetry)); + + 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) { + missionProcessor.init(ammo, telemetry.z); + mpInitialized = true; + } + + dlink::Control control{0.0f, 0.0f}; + if (mpInitialized) { + MissionResult result = missionProcessor.process(telemetry, targetPositions, targetCount); + + if (result.shouldDrop && !dropped) { + LOG("DROP!"); + gpio.signalDrop(); + dropped = true; + } + + DroneTelemetry droneTelemetry = mapDlinkTelemetry(telemetry); + control = droneController.computeControl(droneTelemetry, result.aimPoint); + } + + uint8_t out[64]; + 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); + LOG("AMMO: " << ammo.name << " mass=" << ammo.mass << " drag=" << ammo.drag << " lift=" << ammo.lift + << " hitRadius=" << ammo.hitRadius << " targets=" << (int)ammo.nTargets); + break; + } + + case dlink::PKT_TARGET: { + dlink::TargetPos targetPosition; + std::memcpy(&targetPosition, payload, sizeof(targetPosition)); + + if (targetPosition.id < targetPositions.size()) { + targetPositions[targetPosition.id] = targetPosition; + } + break; + } + } + } } - 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)); - } - - targetsPtr->start(); - physicsPtr->start(); - missionProcessor.start(); - - missionThread.join(); - - physicsPtr->stop(); - targetsPtr->stop(); - - physicsThread.join(); - providerThread.join(); - - 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 9250968..c7cd60e 100644 --- a/drone_simulation/src/mission_processor.cpp +++ b/drone_simulation/src/mission_processor.cpp @@ -1,226 +1,163 @@ #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) -{ - this->droneState = std::make_unique(); -} +#include "log.hpp" -auto MissionProcessor::init(std::unique_ptr configLoader) -> void +MissionProcessor::MissionProcessor(std::unique_ptr solver, const DroneConfig& config) + : ballisticSolver(std::move(solver)) + , droneConfig(config) { - configLoader->load(); - this->droneConfig = configLoader->getConfig(); - this->ammo = configLoader->getAmmoParams(); - - BallisticResult ballisticResult = this->ballisticSolver->solve(this->ammo); - this->ammoFlightTime = ballisticResult.flightTime; - this->ammoHorizontalFlightDistance = ballisticResult.hDist; - this->acceleration = this->droneConfig.attackSpeed * this->droneConfig.attackSpeed / (2.0f * this->droneConfig.accelPath); } -auto MissionProcessor::getCurrentStep() -> int +auto MissionProcessor::init(const dlink::AmmoCfg& ammoCfg, float altitude) -> void { - return this->currentStep; + this->ammo = {.name = std::string(ammoCfg.name), .mass = ammoCfg.mass, .drag = ammoCfg.drag, .lift = ammoCfg.lift}; + this->hitRadius = ammoCfg.hitRadius; + + this->droneConfig.altitude = altitude; + this->ammoFlightTime = getAmmoFlightTime(this->droneConfig, this->ammo); + this->ammoHorizontalFlightDistance = getAmmoHorizontalFlightDistance(this->droneConfig.attackSpeed, this->ammoFlightTime, this->ammo); + + this->initialized = true; } -auto MissionProcessor::step() -> SimulationStep +auto MissionProcessor::process(const dlink::Telemetry& telemetry, + const std::vector& targets, + int targetCount) -> MissionResult { + if (!this->initialized || targetCount <= 0) { + return {Coord{0, 0}, Coord{0, 0}, false, -1}; + } + + 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; + + if (this->prevTime >= 0.0f && !this->prevTargetPositions.empty()) { + float dt = currentTime - this->prevTime; + if (dt > 1e-6f) { + 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}; + this->targetVelocities[i] = (current - this->prevTargetPositions[i]) / dt; + } + } + } + else { + this->targetVelocities.resize(targetCount, {0.0f, 0.0f}); + } + + this->prevTargetPositions.resize(targetCount); + for (int i = 0; i < targetCount && i < (int)targets.size(); ++i) { + this->prevTargetPositions[i] = {targets[i].x, targets[i].y}; + } + this->prevTime = currentTime; + int bestTargetIndex = -1; float bestTotalTime = INFINITY; - Coord bestFireCoord = {.x = 0, .y = 0}; - DroneTelemetry droneTelemetry = this->dronePhysics->getTelemetry(); + Coord bestFireCoord = {0, 0}; + Coord bestAimCoord = {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)this->targetVelocities.size()) ? this->targetVelocities[i] : Coord{0, 0}; Coord fireCoord; - float totalTime = this->evaluateTarget(i, droneTelemetry, 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) { - ++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 {Coord{0, 0}, Coord{0, 0}, false, -1}; } - Coord deltaToFire = bestFireCoord - droneTelemetry.pos; - Coord dirVec = normalize(deltaToFire); - float newDirection = atan2(dirVec.y, dirVec.x); - - 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); + Coord bestTargetPos = {targets[bestTargetIndex].x, targets[bestTargetIndex].y}; + Coord bestVelocity = (bestTargetIndex < (int)this->targetVelocities.size()) ? this->targetVelocities[bestTargetIndex] : Coord{0, 0}; + bool shouldDrop = this->checkDrop(droneTelemetry, bestTargetPos, bestVelocity); 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(); + return {bestFireCoord, bestAimCoord, shouldDrop, bestTargetIndex}; } -auto MissionProcessor::getSteps() const -> const std::vector& +auto MissionProcessor::evaluateTarget(int targetIndex, + const DroneTelemetry& telemetry, + const Coord& targetPos, + const Coord& targetVelocity, + Coord& outFireCoord, + Coord& outAimCoord) -> float { - return this->simulationSteps; -} - -void MissionProcessor::run() -{ - 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)); - } -} + float currentHorizDist = getAmmoHorizontalFlightDistance(telemetry.speed, this->ammoFlightTime, this->ammo); -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); + float distanceToTarget = getDistanceToTarget(targetPos, telemetry.pos); if (distanceToTarget < 1e-3f) { return INFINITY; } - float ratio = getRatio(distanceToTarget, this->ammoHorizontalFlightDistance); - Coord fireCoord = getFireCoords(target, telemetry.pos, ratio); + float ratio = getRatio(distanceToTarget, currentHorizDist); + Coord fireCoord = getFireCoords(targetPos, telemetry.pos, ratio); float distanceToFire = getDistanceToTarget(fireCoord, telemetry.pos); - float droneToFireTime = distanceToFire / this->droneConfig.attackSpeed; + float droneToFireTime = distanceToFire / std::max(telemetry.speed, 1.0f); float totalTimeRough = droneToFireTime + this->ammoFlightTime; - Coord predictedTarget = t.pos + t.velocity * 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->droneConfig.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); if (newRatio < 0) { - totalTime += getManeuveringTime(newDistanceToTarget, - this->ammoHorizontalFlightDistance, - this->droneConfig.attackSpeed, - this->acceleration, - this->droneConfig.angularSpeed); + 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::getSimulationStep(int targetIndex, const Coord& fireCoord, const DroneTelemetry& telemetry) -> SimulationStep +auto MissionProcessor::checkDrop(const DroneTelemetry& telemetry, const Coord& targetPos, const Coord& targetVelocity) -> bool +{ + float currentHorizDist = getAmmoHorizontalFlightDistance(telemetry.speed, this->ammoFlightTime, this->ammo); + Coord bombLandCoord = getBombLandCoord(telemetry, currentHorizDist); + + Coord predictedTarget = targetPos + targetVelocity * this->ammoFlightTime; + float bombMissDistance = getDistanceToTarget(bombLandCoord, predictedTarget); + + 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; +} + +auto MissionProcessor::reset() -> void { - Coord bombLandCoord = getBombLandCoord(telemetry, this->ammoHorizontalFlightDistance); - - Target target = this->targets->getTarget(targetIndex); - Coord targetAtImpact = target.pos + target.velocity * this->ammoFlightTime; - - float bombMissDistance = getDistanceToTarget(bombLandCoord, targetAtImpact); - Coord dir = {.x = std::cos(telemetry.direction), .y = std::sin(telemetry.direction)}; - Coord aimPoint = telemetry.pos + dir * this->ammoHorizontalFlightDistance; - - bool isHit = bombMissDistance < this->droneConfig.hitRadius || bombLandCoord == targetAtImpact; - - return {.hit = isHit, - .target = targetIndex, - .droneSpeed = telemetry.speed, - .droneDirection = telemetry.direction, - .dropPoint = fireCoord, - .aimPoint = aimPoint, - .predictedTarget = targetAtImpact, - .dronePosition = telemetry.pos, - .droneState = DroneStates::Stopped}; + this->prevTargetIndex = -1; + this->prevTime = -1.0f; + this->prevTargetPositions.clear(); + this->targetVelocities.clear(); + this->initialized = false; } -auto MissionProcessor::hasNext() -> bool +auto MissionProcessor::changeSolver(std::unique_ptr ballisticSolver) -> void { - return this->currentStep <= this->MAX_STEPS && !this->hit; -} \ No newline at end of file + this->ballisticSolver = std::move(ballisticSolver); +} 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..b8d9c28 --- /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* buffer, size_t maxByteCount) -> int +{ + // read() поверне: + // > 0 — кількість прочитаних байтів + // -1 — помилка або "даних нема" (errno == EAGAIN, бо O_NONBLOCK) + // В головному циклі ми просто ігноруємо -1 і пробуємо знову. + return read(this->fd, buffer, maxByteCount); +} + +auto UartPort::writeBytes(const uint8_t* buffer, size_t byteCount) -> void +{ + // write() кладе байти в буфер ядра, звідки вони підуть на дріт. + // Для коротких пакетів (8-16 байт) це відбувається миттєво. + write(this->fd, buffer, byteCount); +} \ No newline at end of file diff --git a/drone_simulation/src/simulation.cpp b/drone_simulation/src/simulation.cpp index 28a6406..6711990 100644 --- a/drone_simulation/src/simulation.cpp +++ b/drone_simulation/src/simulation.cpp @@ -7,21 +7,6 @@ using json = nlohmann::json; -#define ENABLE_LOG 0 -#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; } @@ -278,4 +261,13 @@ float length(const Coord& coord) Coord normalize(const Coord& coord) { return coord / length(coord); +} + +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