diff --git a/pilot.cpp b/pilot.cpp
new file mode 100644
index 0000000..507895f
--- /dev/null
+++ b/pilot.cpp
@@ -0,0 +1,109 @@
+#include "Tasks/Task.h"
+#include <cmath>
+#include <vector>
+
+// Данная функция выполняется только один раз при запуске программы
+// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
+
+// Настройки
+constexpr float Ki = 0.03f;
+constexpr float target_height = 65.f;
+constexpr float waypoint_radius = 1.5f;
+constexpr float pitch_constraint = 20.f;
+constexpr float yaw_constraint = 20.f;
+
+struct WayPoint {
+    float x;
+    float z;
+};
+
+std::vector<WayPoint> wayPoints = {
+    WayPoint{.x = -480.f, .z = 85.f},
+    WayPoint{.x = -30.f, .z = 500.f},
+    WayPoint{.x = 150.f, .z = 330.f},
+    WayPoint{.x = 150.f, .z = 5.f},
+};
+
+void Task_solution::setup(HardwareSerial *a_Serial) {
+    debug_serial = a_Serial;
+    _Point_Index = 0;
+}
+
+float distance(const float x1, const float y1, const float x2, const float y2) {
+    return static_cast<float>(sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2)));
+}
+
+bool isEqual(const float a, const float b, const float accuracy) {
+    return abs(a - b) <= accuracy;
+}
+
+/*
+### УЧАСТНИКАМ ОЛИМПИАДЫ  -  Основная функция отправки пакета значений крена и тангажа (град) ####
+Данная функция осуществляет циклический обмен данных с симулятором с частотой 100 Гц.
+*/
+SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
+    /* a_telemetry - структура данных, полученная с симулятора. Она содержит текущие параметры БЛА
+    a_telemetry.L - координата Х (север) [м]
+    a_telemetry.Z - координата Z (восток) [м]
+    a_telemetry.H - координата Y (высота) [м]
+    a_telemetry.Psi - угол курса [град]
+    a_telemetry.Gam - Угол крена [град]
+    a_telemetry.Tan - Угол тангажа [град]
+    a_telemetry.V -  Скорость полёта БЛА [м/с]
+    a_telemetry.Vx1 - Продольная скорость [м/с]
+    a_telemetry.Vz1 - Поперечная скорость [м/с]
+    a_telemetry.Vy1 - Вертикальная скорость [м/с]
+    a_telemetry.wx - Угловая скорость вокруг продольной оси [1/с]
+    a_telemetry.wy - Угловая скорость вокруг вертикальной оси [1/с]
+    a_telemetry.wz - Угловая скорость вокруг поперечной оси [1/с]
+    */
+
+    // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4
+    //
+    SignalBody _ans; // Структура которая отсылается на симулятор
+    // Проверить близость БЛА к текущей путевой точке. Изменить путевую точку при необходимости
+    // _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
+    const float target_x = wayPoints[_Point_Index].x;
+    const float target_y = wayPoints[_Point_Index].z;
+    const auto target_psi = static_cast<float>(
+        -degrees(atan2(target_y - a_telemetry.Z, target_x - a_telemetry.L))
+    );
+
+
+    _ans.Gamma_direct = GammaReg(a_telemetry.Psi, target_psi);
+    _ans.Tang_direct = HeightReg(a_telemetry.H, a_telemetry.Vy1, target_height);
+
+    const float dist_to_target = distance(a_telemetry.L, a_telemetry.Z, target_x, target_y);
+    if (dist_to_target <= waypoint_radius) {
+        // Проверяем расстояние до цели (радиус 5 метров)
+        if (_Point_Index < wayPoints.size() - 1) {
+            _Point_Index++;
+        }
+    }
+
+    // Отправляем команды на симулятор
+    // БЛА будет пытаться выдерживать заданные углы крена и тангажа
+    return _ans;
+}
+
+float roll_err_last = 0.f;
+float integral_err = 0.05f;
+
+float Task_solution::GammaReg(float Psi, float Psi_dir) {
+    float roll_err = AngDefines(Psi - Psi_dir);
+    integral_err += roll_err * 10.f;
+    integral_err = constrain(integral_err, -50.f, 50.f);
+    float gamma_cmd = 2.0f * roll_err + (Ki * integral_err) + 35.f * (roll_err - roll_err_last);
+    roll_err_last = roll_err;
+    gamma_cmd = constrain(gamma_cmd, -yaw_constraint, yaw_constraint);
+    return gamma_cmd;
+}
+
+float Task_solution::HeightReg(float Yg, float Vy, float Hz) {
+    constexpr float K_H = 0.56f;
+    constexpr float K_vy_1 = 7.7591f;
+    constexpr float K_vy_2 = 9.8941f;
+    const float kh1 = constrain(K_H * (Hz - Yg), -7.5f, 7.5f);
+    const float Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -pitch_constraint, pitch_constraint);
+    return Pitch_direct;
+}