Инженерный тур. 3 этап

Предлагаемый алгоритм решения:

1. Разработать алгоритм поиска красного объекта на изображении.
2. Найти луч направления на объект.
3. Перевести направление луча в глобальную систему координат с помощью матрицы поворота.
4. Используя информацию о положении камеры и высоте искомого объекта, найти его координаты.

Пример программы-решения

Ниже представлено решение на языке Python.

import cv2
import numpy as np
import pandas as pd
class SpatialEstimator:
    """
    Класс для обработки телеметрии и изображений с целью определения мировых координат красного объекта.
    """
    def __init__(self, ph=3.9e-06, pw=4.6e-06, f=0.1, resolution=(640, 380)):
        """
        Инициализация параметров камеры и объекта.
        :param ph: Высота пикселя (м).
        :param pw: Ширина пикселя (м).
        :param f: Фокусное расстояние (м).
        :param resolution: Кортеж (ширина, высота) изображения.
        :param object_Z: Известная Z-координата объекта в мировых координатах ….
        """
        self.ph = ph
        self.pw = pw
        self.f = f
        self.resolution = resolution
    def read_telemetry(self, file_path):
        """
        Считывает телеметрию из CSV и возвращает первую строку данных.
        Ожидается наличие столбцов: X, Y, Z, Yaw, Pitch, Roll.
        :param file_path: Путь к файлу CSV.
        :return: Первая строка DataFrame в виде Series.
        """
        df = pd.read_csv(file_path)
        return df.iloc[0]
    def detect_red_object(self, image):
        """
        Находит красный объект на изображении.
        Преобразует изображение в HSV, проводит пороговую фильтрацию по красному цвету
        и вычисляет центр объекта по моментам.
        :param image: Изображение в формате BGR.
        :return: Кортеж (u, v) с координатами центра объекта или None, если объект не найден.
        """
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # Диапазоны для красного цвета (учитываются два диапазона по Hue)
        lower_red1 = np.array([0, 100, 100])
        upper_red1 = np.array([10, 255, 255])
        lower_red2 = np.array([160, 100, 100])
        upper_red2 = np.array([180, 255, 255])
        mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
        mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
        mask = cv2.bitwise_or(mask1, mask2)
        # Убираем шумы (открытие и закрытие)
        kernel = np.ones((3, 3), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if not contours:
            return None
        # Выбираем самый большой контур
        largest_contour = max(contours, key=cv2.contourArea)
        M = cv2.moments(largest_contour)
        if M["m00"] == 0:
            return None
        cx = int(M["m10"] / M["m00"])
        cy = int(M["m01"] / M["m00"])
        return [cx, cy]
    def rotation_matrix(self, yaw, pitch, roll):
        """
        Формирует матрицу поворота по трём углам (Yaw, Pitch, Roll).
        Углы задаются в градусах. Порядок поворотов: сначала курс (Yaw, вокруг оси Z),
        затем тангаж (Pitch, вокруг оси Y), затем крен (Roll, вокруг оси X).
        :param yaw: Угол курса.
        :param pitch: Угол тангажа.
        :param roll: Угол крена.
        :return: Матрица поворота 3x3.
        """
        # Переводим углы в радианы
        yaw = np.deg2rad(-yaw)
        pitch = np.deg2rad(pitch)
        roll = np.deg2rad(roll)
        # Матрица поворота вокруг оси Z (курс)
        Rz = np.array([
            [np.cos(yaw), -np.sin(yaw), 0],
            [np.sin(yaw),  np.cos(yaw), 0],
            [0, 0, 1]
        ])
        # Матрица поворота вокруг оси Y (тангаж)
        Ry = np.array([
            [ np.cos(pitch), 0, np.sin(pitch)],
            [ 0, 1, 0],
            [-np.sin(pitch), 0, np.cos(pitch)]
        ])
        # Матрица поворота вокруг оси X (крен)
        Rx = np.array([
            [1, 0, 0],
            [0, np.cos(roll), -np.sin(roll)],
            [0, np.sin(roll),  np.cos(roll)]
        ])
        # Итоговая матрица поворота
        R = Rz @ Ry @ Rx
        return R
    def compute_world_coordinates(self, telemetry, pixel_coords):
        """
        Вычисляет мировые координаты объекта.
        :param telemetry: Объект с данными телеметрии (поля: X, Y, Z, Yaw, Pitch, Roll, ObjectHeight).
        :param pixel_coords: Кортеж (u, v) с координатами объекта на изображении.
        :return: Массив с мировыми координатами объекта.
        """
        # Убедитесь, что ObjectHeight доступен РІ telemetry
        try:
            object_Z = telemetry['ObjectHeight']
        except KeyError:
            print("Ошибка: 'ObjectHeight' отсутствует в данных телеметрии.")
            return None # Или обработать иначе
        u, v = pixel_coords
        width, height = self.resolution
        cx = width / 2
        cy = height / 2
        # 1. Луч в координатах камеры (X вправо, Y вниз, Z вперед)
        x_sensor = (u - cx) * self.pw
        y_sensor = (v - cy) * self.ph  # Убедитесь, что v растет вниз
        ray_cam = np.array([x_sensor, y_sensor, self.f])
        # 2. Матрица поворота Тело -> Мир
        R_body2world = self.rotation_matrix(telemetry['Yaw'], telemetry['Pitch'], telemetry['Roll'])
        # 3. Матрица поворота Камера -> Тело (Фиксированная)
        #    Предполагаем ту же ориентацию, что в первом скрипте:
        #    Ось X камеры совпадает с осью X тела
        #    Ось Y камеры противоположна оси Y тела
        #    Ось Z камеры (оптическая) противоположна оси Z тела
        R_cam2body = np.array([
            [1,  0,  0],
            [0, -1,  0],
            [0,  0, -1]
        ])
        # ВНИМАНИЕ: Если установка камеры другая, эта матрица должна быть изменена!
        # 4. Полная матрица поворота Камера -> Мир
        R_cam2world = R_body2world @ R_cam2body
        # 5. Преобразуем луч в мировую систему координат
        ray_world = R_cam2world @ ray_cam
        # print(f"Ray Cam: {ray_cam}") # Отладка
        # print(f"Ray World: {ray_world}") # Отладка
        # 6. Положение камеры в мировой системе координат
        cam_pos = np.array([telemetry['X'], telemetry['Y'], telemetry['Z']])
        # 7. Решаем уравнение прямой для пересечения с Z = object_Z
        # Проверка деления на ноль (луч параллелен плоскости Z)
        if abs(ray_world[2]) < 1e-9:
            print("Предупреждение: Луч почти параллелен плоскости Z=const.")
            return None # Или вернуть очень большие координаты / NaN
        t = (object_Z - cam_pos[2]) / ray_world[2]
        # Можно добавить проверку t > 0 (пересечение перед камерой)
        if t < 0:
             print("Предупреждение: Точка пересечения находится позади камеры.")
             # Можно вернуть None или оставить как есть, в зависимости от требований
        # 8. Вычисляем мировые координаты точки пересечения
        obj_world = cam_pos + t * ray_world
        return obj_world
    def process(self, telemetry_file, image_file):
        """
        Полный процесс: считывание телеметрии, загрузка изображения,
        обнаружение красного объекта и вычисление его мировых координат.
        :param telemetry_file: Путь к файлу телеметрии (CSV).
        :param image_file: Путь к файлу изображения.
        :return: Мировые координаты объекта или None, если объект не найден или ошибка.
        """
        try:
            telemetry = self.read_telemetry(telemetry_file)
        except FileNotFoundError:
            print(f" Ошибка: Файл телеметрии не найден: {telemetry_file}")
            return None
        except Exception as e:
            print(f" Ошибка при чтении телеметрии: {e}")
            return None
        image = cv2.imread(image_file)
        if image is None:
             print(f" Ошибка: Не удалось загрузить изображение: {image_file}")
             return None
        red_object_pixel = self.detect_red_object(image)
        if red_object_pixel is None:
            print("Красный объект не найден на изображении.")
            return None
        obj_world = self.compute_world_coordinates(telemetry, red_object_pixel)
        return obj_world
# --- Остальной код остается без изменений ---
if __name__ == "__main__":
    estimator = SpatialEstimator(ph=3.9e-06, pw=4.6e-06, f=0.1) # Используем заданные параметры
    telemetry_file = 'telemetry3.csv'
    image_file = 'img3.png'
    obj_world = estimator.process(telemetry_file, image_file)
    if obj_world is not None:
        print("\n Вычисленные мировые координаты объекта
:")
        print(f"X={obj_world[0]:.4f}, Y={obj_world[1]:.4f}, Z={obj_world[2]:.4f}")

Предлагаемый алгоритм решения:

1. Выбрать структуру законов управления в каналах крена и тангажа.
2. Определить необходимые значения угла курса и высоты для достижения точки доставки груза.
3. Рассчитать момент времени для сброса груза.
4. Отработать разработанный алгоритм на симуляторе.
5. Проанализировать полученный результат и доработать алгоритмы управления.

Пример программы-решения

#include "Tasks/Task.h"
// Данная функция выполняется только один раз при запуске программы
// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
void Task_solution::setup(HardwareSerial* a_Serial)
{
  debug_serial = a_Serial;
  // Примеры вывода в отладочную консоль
  printtoDebugSerial("Hello team!");
  printtoDebugSerial(String(3.1415926));
  // Путевые точки в произвольном порядке
  _PointsArray[0] = FlyPoints(341, 541, 45);
}
/*
### УЧАСТНИКАМ ОЛИМПИАДЫ  -  Основная функция отправки пакета значений крена и тангажа (град) ####
Данная функция осуществляет циклический обмен данных с симулятором с частотой 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/с]
  */
  // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ
  //
  SignalBody _ans;  // Структура которая отсылается на симулятор
  // Проверить близость БВС к текущей путевой точке. изменить путевую точку при необходимости
  // _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
  _Point_Index = 0;
  // Через координаты путевой точки и текущего местоположения БВС найти направление на путевую точку (пеленг)
  _ans.Gamma_direct =  PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, a_telemetry.Psi);
  // Рассчитать необходимое изменение угла курса
  // Рассчитать требуемый угол крена (для регулирования курса)
  // На основе текущей высоты БВС и заданной высоты путевой точки рассчитать необходимое изменение высоты
  _ans.Tang_direct =  PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, _Point_Index);
  // Рассчитать требуемый угол тангажа (для регулирования высоты
)
 
 
  float delX = _PointsArray[0].North - a_telemetry.L;
  float delY =  _PointsArray[0].East - a_telemetry.Z;
  float delZ =  _PointsArray[0].Height - a_telemetry.H;
  float dist = sqrtf(pow(delX, 2) + pow(delY, 2));
 
  if (dist < 106.5)
  {
    cargo_drop = true;
    _PointsArray[0] = FlyPoints(341, 541, 60);
  }
  // Отправляем команды на симулятор
  // БВС будет пытаться выдерживать заданные углы крена и тангажа
  return _ans;
}
float roll_err_last = 0;
float Task_solution::GammaReg(float Psi, float Psi_dir)
{
  float Kpsi = 1.6;
  float roll_err = AngDefines(Psi - Psi_dir);
  float gamma_cmd = 3.8 * roll_err + 45 * (roll_err - roll_err_last);
  roll_err_last = roll_err;
  gamma_cmd = constrain(gamma_cmd, -30, 30);
  return gamma_cmd;
}
float Task_solution::HeightReg(float Yg, float Vy, float Hz)
{
  float K_H = 0.72;
  float K_vy_1 = 7.7591;
  float K_vy_2 = 9.8941;
  float kh1 = constrain(K_H * (Hz - Yg), -7.5, 7.5);
  float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20);
  return _Pitch_direct;
}
float Task_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, float Psi)
{
  float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt);
  Psi_cmd *= 57.3;
  return GammaReg(Psi, Psi_cmd);
}
float Task_solution::PointsFlyGam(const size_t& a_Point_index, float _Xt, float _Yt, float Psi)
{
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi);
}
float Task_solution::PointsFlyTan(float H, float Vy, const size_t& a_Point_index)
{
  float H_z = _PointsArray[_Point_Index].Height;
  return HeightReg(H, Vy, H_z);
}
size_t Task_solution::GetNowPointIndex(float X, float Z, float H)
{
  size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]);
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  float Yg_cmd = _PointsArray[_Point_Index].Height;
  float delX = Xg_cmd - X;
  float delY = Zg_cmd - Z;
  float delZ = Yg_cmd - H;
  float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2));
  // Calculate crosstrack error
  float x_og = 0.0;
  float z_og = 0.0;
  if (_Point_Index > 0)
  {
    x_og = _PointsArray[_Point_Index - 1].North;
    z_og = _PointsArray[_Point_Index - 1].East;
  }
  else
  {
    x_og = _PointsArray[max_index - 1].North;
    z_og = _PointsArray[max_index - 1].East;
  }
  float relx = X - x_og;
  float rely = Z - z_og;
  float delx = Xg_cmd - x_og;
  float dely = Zg_cmd - z_og;
  float U = (relx * delx + rely * dely) / (delx * delx + dely * dely);
  float cte = (rely * delx - relx * dely) / (delx * delx + dely * dely);
  if (Size < 7.0)
  {
    _Point_Index++;
  }
  if (_Point_Index >= max_index)
  {
    _Point_Index = 0;
  }
  return _Point_Index;
}

Предлагаемый алгоритм решения:

1. Отладить структуру законов управления в каналах крена и тангажа.
2. Выбрать траекторию движения БВС (последовательность путевых точек) с учетом заданного ограничения по времени.
3. Отработать разработанный алгоритм на симуляторе.
4. Проанализировать полученный результат и доработать алгоритмы управления.

Пример программы-решения

#include "Tasks/Task.h"
// Данная функция выполняется только один раз при запуске программы
// (при подаче питания на плату, перепрошивке, или нажатии кнопки  reset)
void Task_solution::setup(HardwareSerial* a_Serial)
{
  debug_serial = a_Serial;
  // Примеры вывода в отладочную консоль
  printtoDebugSerial("Hello team!");
  printtoDebugSerial(String(3.1415926));
  // Путевые точки в произвольном порядке
  _PointsArray[0] = FlyPoints(-550, 60, 65);
  // _PointsArray[1] = FlyPoints(310, -110, 60);
  _PointsArray[2] = FlyPoints(-500, -135, 65);
  // _PointsArray[2] = FlyPoints(-329, -71, 65);
  _PointsArray[3] = FlyPoints(65, 323, 65);
  _PointsArray[4] = FlyPoints(-82, -106, 65);
  _PointsArray[5] = FlyPoints(59, -247, 65);
  _PointsArray[6] = FlyPoints(347, 41, 65);
  _PointsArray[7] = FlyPoints(488, -100, 65);
  _PointsArray[8] = FlyPoints(200, -388, 65);
}
/*
### УЧАСТНИКАМ ОЛИМПИАДЫ  -  Основная функция отправки пакета значений крена и тангажа (град) ####
Данная функция осуществляет циклический обмен данных с симулятором с частотой 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/с]
  */
  // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ
  //
  SignalBody _ans;  // Структура которая отсылается на симулятор
  // Проверить близость БВС к текущей путевой точке. Изменить путевую точку при необходимости
  _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
  // _Point_Index = 0;
  // Через координаты путевой точки и текущего местоположения БВС найти направление на путевую точку (пеленг)
  _ans.Gamma_direct = PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, a_telemetry.Psi);
  // Рассчитать необходимое изменение угла курса
  // Рассчитать требуемый угол крена (для регулирования курса)
  // На основе текущей высоты БВС и заданной высоты путевой точки рассчитать необходимое изменение высоты
  _ans.Tang_direct =  PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, _Point_Index);
  // Рассчитать требуемый угол тангажа (для регулирования высоты)
 
 
  // float delX = _PointsArray[0].North - a_telemetry.L;
  // float delY =  _PointsArray[0].East - a_telemetry.Z;
  // float delZ =  _PointsArray[0].Height - a_telemetry.H;
  // float dist = sqrtf(pow(delX, 2) + pow(delY, 2));
 
  // if (dist < 106.5)
  // {
  //   cargo_drop = true;
  //   _PointsArray[0] = FlyPoints(-300, 300, 60);
  // }
  // Отправляем команды на симулятор
  // БВС будет пытаться выдерживать заданные углы крена и тангажа
  return _ans;
}
float roll_err_last = 0;
float Task_solution::GammaReg(float Psi, float Psi_dir)
{
  float Kpsi = 1.6;
  float roll_err = AngDefines(Psi - Psi_dir);
  float gamma_cmd = 3.8 * roll_err + 45 * (roll_err - roll_err_last);
  roll_err_last = roll_err;
  gamma_cmd = constrain(gamma_cmd, -20, 20);
  return gamma_cmd;
}
float Task_solution::HeightReg(float Yg, float Vy, float Hz)
{
  float K_H = 0.72;
  float K_vy_1 = 7.7591;
  float K_vy_2 = 9.8941;
  float kh1 = constrain(K_H * (Hz - Yg), -7.5, 7.5);
  float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20);
  return _Pitch_direct;
}
float Task_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, float Psi)
{
  float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt);
  Psi_cmd *= 57.3;
  return GammaReg(Psi, Psi_cmd);
}
float Task_solution::PointsFlyGam(const size_t& a_Point_index, float _Xt, float _Yt, float Psi)
{
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi);
}
float Task_solution::PointsFlyTan(float H, float Vy, const size_t& a_Point_index)
{
  float H_z = _PointsArray[_Point_Index].Height;
  return HeightReg(H, Vy, H_z);
}
size_t Task_solution::GetNowPointIndex(float X, float Z, float H)
{
  size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]);
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  float Yg_cmd = _PointsArray[_Point_Index].Height;
  float delX = Xg_cmd - X;
  float delY = Zg_cmd - Z;
  float delZ = Yg_cmd - H;
  float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2));
  // Calculate crosstrack error
  float x_og = 0.0;
  float z_og = 0.0;
  if (_Point_Index > 0)
  {
    x_og = _PointsArray[_Point_Index - 1].North;
    z_og = _PointsArray[_Point_Index - 1].East;
  }
  else
  {
    x_og = _PointsArray[max_index - 1].North;
    z_og = _PointsArray[max_index - 1].East;
  }
  float relx = X - x_og;
  float rely = Z - z_og;
  float delx = Xg_cmd - x_og;
  float dely = Zg_cmd - z_og;
  float U = (relx * delx + rely * dely) / (delx * delx + dely * dely);
  float cte = (rely * delx - relx * dely) / (delx * delx + dely * dely);
  if (Size < 7.0)
  {
    _Point_Index++;
  }
  if (_Point_Index >= max_index)
  {
    _Point_Index = 0;
  }
  return _Point_Index;
}

Пример программы-решения

//Инициализация библиотек
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <ESP32Servo.h> // Библиотека для работы с сервоприводом на ESP32
#include <WiFi.h>
const char* loraAP_ssid = "LORA_AP";            // Сеть для отправки данных
const char* loraAP_pass = "12345678";
WiFiClient loraClient;
#if defined(ARDUINO_SUPER_MINI_ESP32C3)
  static uint8_t ledPin = BUILTIN_LED;   // LED connected to digital pin
#else
  static uint8_t ledPin = 8;             // LED connected to digital pin
#endif
static uint8_t ledOn = LOW;
#define DELTA 10
#define DELAY 50
unsigned long LEDdelaytime = 0;
int delta = 5;
int fade = 0;
void pulse(void) {
  if (millis() - LEDdelaytime > DELAY) {
    fade += delta;
    if (fade <= 0) {
      fade = 0;
      delta = DELTA;
    } else if (fade >= 255) {
      fade = 255;
      delta = - DELTA;
    }
    analogWrite(ledPin, fade);
    LEDdelaytime = millis();
  }
}
Servo myServo; // Сервопривод
// Переменные для хранения смещений
float accelOffsetX = 0;
float accelOffsetY = 0;
float accelOffsetZ = 0;
const int buttonPin = 4;     // Пин для оптического датчика
const int servoPin = 3;            // Пин для сервопривода
const int buzzerPin = 2;           // Пин для пищалки
int16_t ax, ay, az;
int16_t gx, gy, gz;
bool objectDropped = false;         //Состояние сброса груза
unsigned long dropTime = 0;         //Время сброса
float maxAcceleration = 0;          //Максимальное значение акселерометра
const int numReadings = 7;

float readings1[numReadings];       // данные, считанные с входного аналогового контакта
float readings2[numReadings];       // данные, считанные с входного аналогового контакта
int sss1 = 0;                       // индекс для значения, которое считывается в данный момент
float total1 = 0;                   // суммарное значение
float average1 = 0;                 // среднее значение

int sss2 = 0;                       // индекс для значения, которое считывается в данный момент
float total2 = 0;                   // суммарное значение
float average2 = 0;                 // среднее значение
int K=0;
float S2=0;
float S3=0;
float S4=0;
float XX=0;
float YY=0;
float ZZ=0;
float totalAbs = 0;
Adafruit_MPU6050 mpu1;
void calibrateMPU6050() {
  Serial.println("Начало калибровки MPU6050...");
  Serial.println("Убедитесь, что датчик неподвижен!");
  int calibrationSamples = 1000; // Количество выборок для калибровки
  float totalAccelX = 0, totalAccelY = 0, totalAccelZ = 0;
  for (int i = 0; i < calibrationSamples; i++) {
    sensors_event_t a, g, temp;
    mpu1.getEvent(&a, &g, &temp);
    // Суммируем значения акселерометра
    totalAccelX += a.acceleration.x;
    totalAccelY += a.acceleration.y;
    totalAccelZ += a.acceleration.z;
    totalAbs += sqrt(a.acceleration.x*a.acceleration.x + a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z);
    delay(5); // Задержка для стабильности
  }
  // Вычисляем средние значения смещений
  accelOffsetX = totalAccelX / calibrationSamples;
  accelOffsetY = totalAccelY / calibrationSamples;
  accelOffsetZ = totalAccelZ / calibrationSamples;
  totalAbs = totalAbs / calibrationSamples;
  // Выводим результаты калибровки
  Serial.println("Калибровка завершена!");
  Serial.print("Смещение по X: "); Serial.println(accelOffsetX);
  Serial.print("Смещение по Y: "); Serial.println(accelOffsetY);
  Serial.print("Смещение по Z: "); Serial.println(accelOffsetZ);
  analogWrite(ledPin, 255);
}
void setup() {
  //Serial.setTxTimeoutMs(0);
  Serial.begin(115200); //38400
  Wire.begin();
  mpu1.begin(104);
  mpu1.setAccelerometerRange(MPU6050_RANGE_16_G);
 
  // Калибровка MPU6050
  calibrateMPU6050();
 
  ESP32PWM::allocateTimer(0); // Выделение таймера для сервопривода
  myServo.setPeriodHertz(50); // Стандартная частота для сервоприводов (50 Гц)
  myServo.attach(servoPin); // Подключение сервопривода
  myServo.write(0);         // Установка начального положения (0 градусов)
 
  // Инициализация пищалки
  pinMode(buzzerPin, OUTPUT);
  // Инициализация оптического датчика
  pinMode(buttonPin, INPUT);

}

float gammaa1 = 0.0;
float acc1[] = {-0.36, -0.09, -1.8};
float acc_scale1 = 1;

float gammaa2 = 0.0;
float acc2[] = {-0.36, -0.09, -1.8};
float acc_scale2 = 1;

void loop() {
  // Считывание показаний оптического датчика
  int buttonValue = digitalRead(buttonPin);
 
  // Если груз ещё не сброшен и датчик переключился с 0 на 1
  if (!objectDropped && buttonValue == 1) {
    objectDropped = true;
    dropTime = millis();
  }
  if (objectDropped)
  {
    // Получение данных с акселерометра
    //код для обработки данных с MPU6050
      sensors_event_t a1, g1, temp1;
      mpu1.getEvent(&a1, &g1, &temp1);

      float acc_x1 = (a1.acceleration.x) * acc_scale1;
      float acc_y1 = (a1.acceleration.y) * acc_scale1;
      float acc_z1 = (a1.acceleration.z) * acc_scale1;
     

  
      //Y=0.995581*(acc_x2 - S2 + 0.9987*Y);
      // XX=0.99*(acc_x1 - S2 + 0.99*XX);
      // S2 = acc_x1;
      // YY=0.99*(acc_y1 - S3 + 0.99*YY);
      // S3 = acc_y1;
      // ZZ=0.99*(acc_z1 - S4 + 0.99*ZZ);
      // S4 = acc_z1;
      float S = (sqrt((acc_x1*acc_x1) + (acc_y1*acc_y1) + (acc_z1*acc_z1)));
      //Serial.println("S");
      //Serial.println(S);
        if (S<3)
        {
          S = 0;
        }
        //Serial.println("S");
        //Serial.println(S);
        //Serial.println(" ");
      // Фиксация максимального ускорения
      S = S - totalAbs;
      if ((S) > maxAcceleration && millis() > 1000) {
      //maxAcceleration = 0;
      maxAcceleration = S;
    }
    //Serial.println(maxAcceleration);
      /*
    if (S > maxAcceleration && millis() - dropTime > 100) {
      maxAcceleration = 0;
      maxAcceleration = S;
    }
    */
    // Через 2 секунды после сброса дать команду на сервопривод
    if (millis() - dropTime > 2000) {
      myServo.write(180);  // Поворот сервопривода на 180 градусов
    }
    // Через 30 секунд после сброса
    if (millis() - dropTime > 10000) {
      // Вывод максимального ускорения в UART
      Serial.print("Max Acceleration: ");
      Serial.println(maxAcceleration);
        // Подключаемся к другой сети
      WiFi.begin(loraAP_ssid, loraAP_pass);
      while (WiFi.status() != WL_CONNECTED) {
        WiFi.begin(loraAP_ssid, loraAP_pass);
        // delay(500);
        // digitalWrite(buzzerPin,LOW); // Выключить пищалку
        pulse();
      }
      if (loraClient.connect("192.168.4.1", 80))
      {
        loraClient.println("acc:" + String(maxAcceleration));
      }
     
     
    //  Serial.print(acc_x1);
    //  Serial.print(",");
    //  Serial.print(acc_y1);
    //  Serial.print(",");
    //  Serial.print(acc_z1);
    //  Serial.println(",");
      // Импульсный сигнал на пищалку
      // static unsigned long lastBuzzerToggle = 0;
      // static bool buzzerState = false;
      // if (millis() - lastBuzzerToggle > 200) {
      //   buzzerState = !buzzerState;
      //   if (buzzerState)
      //   {
      //     digitalWrite(buzzerPin,HIGH); // Включить пищалку
      //   }
      //   else
      //   {
      //     digitalWrite(buzzerPin,LOW); // Выключить пищалку
      //   }
      //   lastBuzzerToggle = millis(); // Обновление времени последнего переключения
       
      // }
    }
  }
  //код для обработки данных с MPU6050

  /* Get new sensor events with the readings */
}

Ниже представлен пример полетного задания.

{
    "fileType": "Plan",
    "geoFence": {
        "circles": [
        ],
        "polygons": [
        ],
        "version": 2
    },
    "groundStation": "QGroundControl",
    "mission": {
        "cruiseSpeed": 15,
        "firmwareType": 3,
        "globalPlanAltitudeMode": 1,
        "hoverSpeed": 5,
        "items": [
            {
                "AMSLAltAboveTerrain": 30,
                "Altitude": 30,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 84,
                "doJumpId": 1,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    0,
                    56.097170899999995,
                    35.8763223,
                    30
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 50,
                "Altitude": 50,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 2,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.097959452143826,
                    35.874978916685166,
                    50
                ],
                "type": "SimpleItem"
            },
            {
                "autoContinue": true,
                "command": 3000,
                "doJumpId": 3,
                "frame": 0,
                "params": [
                    4,
                    0,
                    0,
                    0,
                    0,
                    0,
                    0
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": null,
                "Altitude": 50,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 4,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.09804814731677,
                    35.87444633766345,
                    50
                ],
                "type": "SimpleItem"
            },
            {
                "autoContinue": true,
                "command": 211,
                "doJumpId": 5,
                "frame": 2,
                "params": [
                    1,
                    0,
                    0,
                    0,
                    0,
                    0,
                    0
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 70,
                "Altitude": 70,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 6,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.09831421908459,
                    35.87284858587688,
                    70
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 70,
                "Altitude": 70,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 7,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.09869265231478,
                    35.874126428124725,
                    70
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 70,
                "Altitude": 70,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 8,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.09839044750299,
                    35.876002922561895,
                    70
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 0,
                "Altitude": 0,
                "AltitudeMode": 1,
                "autoContinue": true,
                "command": 21,
                "doJumpId": 9,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    56.097805553443784,
                    35.876140121248255,
                    0
                ],
                "type": "SimpleItem"
            }
        ],
        "plannedHomePosition": [
            56.09779451800145,
            35.87598385677313,
            162
        ],
        "vehicleType": 1,
        "version": 2
    },
    "rallyPoints": {
        "points": [
        ],
        "version": 2
    },
    "version": 1
}

Предлагаемый алгоритм решения:

1. Сформировать траекторию полета БВС; написать код алгоритма траекторного управления для реализации этой траектории.
2. Доработать систему управления БВС.
3. Отладить алгоритм определения координат точки доставки груза.
4. Продумать логику сброса груза после нахождения координат точек доставки
5. Отработать разработанные алгоритмы на симуляторе.

Пример программы-решения

# -*- coding: utf-8 -*-
import socket
from threading import Thread
import time
from math import atan2, cos, sin, sqrt, pi, atan
import cv2
import numpy as np
import uav_state
import serial
import av
import subprocess
# Последние полученные данные телеметрии
last_telemetry = uav_state.uav_state()
running = True
# capture = cv2.VideoCapture()
# container = av.open("rtp-forwarder.sdp",
#                     format="sdp",
#                     options={"protocol_whitelist": "file,udp,rtp"})
ser = serial.Serial(
port='COM8',
baudrate = 115200,
parity = serial.PARITY_NONE,
stopbits = serial.STOPBITS_ONE,
bytesize = serial.EIGHTBITS,
timeout = 0.1 )
# UDP сервер для параллельного получения данных телеметрии
def udp_server_thread():
    host = '127.0.0.1'
    port = 63504
    addr = (host, port)
    server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    server.bind(addr)
    while running:
        d = server.recvfrom(1024)
        received = d[0]
        addr = d[1]
       
        received = received.decode("utf-8")
        received = received.replace("\t\r\n", "")
        #print("---")
        #print(received)
        arr = [float(i) for i in received.split('\t')]
        if len(arr) == 18:
            last_telemetry.update(arr)
    server.close()
def sendNewWaypoint(newx, newz, newh):
    ser.write("wpnt {:.2f} {:.2f} {:.2f} \n".format(newx, newz, newh).encode())
    print("wpnt {:.2f} {:.2f} {:.2f}".format(newx, newz, newh))
def sendDropCargo():
    ser.write("drop \n".encode())
    print("cargo drop attempt!")
def find_surroundings(image, center, radius):
    dots = []
    h, w = image.shape
    for row in range(center[1] - radius, center[1] + radius):
        if row < h and row >= 0:
            dots.append(image[row][center[0]])
    for col in range(center[0] - radius, center[0] + radius):
        if col < w and col >= 0:
            dots.append(image[center[1]][col])
    avg_val = sum(dots) / len(dots)
    return avg_val
# Поток обработки данных с камеры
def camera_processing_thread():
    # Параметры камеры
    F = 6.1 * 1e-3  # Фокусное расстояние
    dv = 7.8 * 1e-6  # высота пикселя
    dh = 9.2 * 1e-6  # ширина пикселя
    xadj = 2
    zadj = -8
    # cap = cv2.VideoCapture("rtp-forwarder.sdp")
    #cap = cv2.VideoCapture("circle.png")
    # cap.set(cv2.CAP_PROP_BUFFERSIZE, 0)
    yellow_lower = np.array([20, 100, 100])
    yellow_upper = np.array([30, 255, 255])
    # lower mask (0-10)
    lower_red1 = np.array([0,150,150])
    upper_red1 = np.array([10,255,255])
   
    # upper mask (170-180)
    lower_red2 = np.array([175,150,150])
    upper_red2 = np.array([180,255,255])
    # Open the SDP file with PyAV.
    # The 'options' dictionary lets you pass extra parameters (like protocol whitelisting).
    container = av.open("rtp-forwarder.sdp",
                        format="sdp",
                        options={"protocol_whitelist": "file,udp,rtp"})
    # Find the video stream (assumes there's at least one video stream in the SDP)
    video_stream = next((s for s in container.streams if s.type == "video"), None)
    if video_stream is None:
        print("No video stream found in the SDP file.")
        exit(1)
    print("ready to search")
    coords = (200, 900)
    stage = 1  # wp number
    evasion = False
    drop = False
    point_one_found = False
    point_two_found = False
    point_one_coords = [-999, -999]
    point_two_coords = [-999, -999]
    # sendNewWaypoint(-245, 40, 80)
    target = (-245, 40)
    drop_dist = 0
    # Основной цикл программы
    # Decode video frames
    for packet in container.demux(video_stream):
        if point_two_found:
            break
        for frame in packet.decode():
            # Convert the frame to a numpy array in a format suitable for OpenCV (BGR)
            # Считанное изображение с бортовой камеры
            img = frame.to_ndarray(format="bgr24")
           
            # Добавьте ваше решение сюда! ---------------
            height, width, channels = img.shape
            tele = last_telemetry
            X = tele.x
            Z = tele.z
            H = tele.h
            Heading = tele.yaw * pi / 180
            img = cv2.GaussianBlur(img, (5, 5), 0)
            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
            yellow_mask = cv2.inRange(hsv, yellow_lower, yellow_upper)
            # mask0 = cv2.inRange(hsv, lower_red1, upper_red1)
            # mask1 = cv2.inRange(hsv, lower_red2, upper_red2)
            # yellow_mask = mask0+mask1
            contours, hierarchy = cv2.findContours(yellow_mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
            maxlen = 0
            if len(contours) > 0:
                truecont = contours[0]
                for icontour in contours:
                    if len(icontour) > maxlen:
                        maxlen = len(icontour)
                        truecont = icontour
            if maxlen > 600: # 560
                print(f"max yellow contour: {maxlen}")
            if maxlen > 1600: # 560
               
                cv2.drawContours(img, truecont, -1, (255,0,0), 2)
                sum_x = 0
                sum_y = 0
                for i in range(0, len(truecont)):
                    sum_x += truecont[i][0][0]
                    sum_y += truecont[i][0][1]
                avg_x = int(round(sum_x / len(truecont)))
                avg_y = int(round(sum_y / len(truecont)))
                dxp = avg_x - width / 2
                dyp = height/2 - avg_y
                dxx = H / F * dh
                dxy = H / F * dv
                # print(f"Pixel size in meters: {dxx}")
                shift_x = dxp * dxx
                shift_y = dyp * dxy
                geo_x = shift_y * cos(Heading) - shift_x * sin(Heading) + X
                geo_y = shift_y * sin(Heading) + shift_x * cos(Heading) + Z
                print("Delivery coords X:{:2f} Z:{:2f}".format(geo_x, geo_y))
                # These are used to avoid multiple detections of the same point
                dist1 = sqrt((geo_x - point_one_coords[0]) ** 2 + (geo_y - point_one_coords[1]) ** 2)
                dist2 = sqrt((geo_x - point_two_coords[0]) ** 2 + (geo_y - point_two_coords[1]) ** 2)
                if not point_one_found:
                    point_one_found = True
                    point_one_coords = [geo_x, geo_y]
                    print("Point 1 found")
                elif point_one_found and not point_two_found and dist1 > 250:
                    point_two_found = True
                    point_two_coords = [geo_x, geo_y]
                    print("Poirnt 2 found")
                    break
            # print(f'{X} {Z}')
            # Отобразить изображение
            # cv2.imshow("Live RTP Stream", img)
            if cv2.waitKey(5) & 0xFF == ord("q"):
                break
        else:
            # Continue if inner loop didn't break
            continue
        # Break outer loop if inner loop was broken.
        break
    cv2.destroyAllWindows()
    print("Both points have been found")
    print(f" point 1: {point_one_coords}")
    print(f" point 2: {point_two_coords}")
    print(f'Approaching first delivery point: {point_one_coords[0]} {point_one_coords[1]} {45}')
    sendNewWaypoint(point_one_coords[0], point_one_coords[1], 48)
    dist = 999
    while dist > 100:
        tele = last_telemetry
        X = tele.x
        Z = tele.z
        dist = sqrt((X - point_one_coords[0]) ** 2 + (Z - point_one_coords[1]) ** 2)
    sendDropCargo()
    sendNewWaypoint(point_one_coords[0], point_one_coords[1], 57)
    time.sleep(4)
    print(f'Approaching second delivery point: {point_two_coords[0]} {point_two_coords[1]} {45}')
    sendNewWaypoint(point_two_coords[0], point_two_coords[1], 48)
    dist = 999
    while dist > 100:
        tele = last_telemetry
        X = tele.x
        Z = tele.z
        dist = sqrt((X - point_two_coords[0]) ** 2 + (Z - point_two_coords[1]) ** 2)
    sendDropCargo()
    sendNewWaypoint(point_two_coords[0], point_two_coords[1], 57)
    time.sleep(4)
    # cv2.destroyAllWindows()
# УЧАСТНИКАМ НТО - ЗАДАЧА  ---------------------------------------
# Основная функция для управления потоками
if __name__ == '__main__':
    # Open RTP livestream through golang script
    # sendNewWaypoint(-245, 40, 80)
    subprocess.Popen(["go", "run", "./main.go"])
    time.sleep(5)
    capture_thr = Thread(target=udp_server_thread, args=[])
    capture_thr.daemon = True
    capture_thr.start()
    process_thr = Thread(target=camera_processing_thread, args=[])
    process_thr.daemon = True
    process_thr.start()
    try:
        while running:
            time.sleep(1)
    except KeyboardInterrupt:
        print('\ninterrupted!')

Ниже представлена программа для ESP32 (выдается шаблон, который может быть использован и изменен участниками на их усмотрение).

#include "Tasks/Task.h"
// Данная функция выполняется только один раз при запуске программы
// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
void Task_solution::setup(HardwareSerial* a_Serial)
{
  debug_serial = a_Serial;
  // Примеры вывода в отладочную консоль
  printtoDebugSerial("Hello team!");
  printtoDebugSerial(String(3.1415926));
  // Путевые точки в произвольном порядке
  _PointsArray[0] = FlyPoints(-700, 0, 65);
  _PointsArray[1] = FlyPoints(-500, -135, 65);
  _PointsArray[2] = FlyPoints(65, 323, 65);
  _PointsArray[3] = FlyPoints(-82, -106, 65);
  _PointsArray[4] = FlyPoints(59, -247, 65);
  _PointsArray[5] = FlyPoints(347, 41, 65);
  _PointsArray[6] = FlyPoints(488, -100, 65);
  _PointsArray[7] = FlyPoints(200, -388, 65);
}
/*
### Основная функция отправки пакета значений крена и тангажа (град) ####
Данная функция осуществляет циклический обмен данных с симулятором с частотой 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);
  // _Point_Index = 0;
  // Через координаты путевой точки и текущего местоположения БВС найти направление на путевую точку (пеленг)
  _ans.Gamma_direct =  PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, a_telemetry.Psi);
  // Рассчитать необходимое изменение угла курса
  // Рассчитать требуемый угол крена (для регулирования курса)
  // На основе текущей высоты БВС и заданной высоты путевой точки рассчитать необходимое изменение высоты
  _ans.Tang_direct =  PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, _Point_Index);
  // Рассчитать требуемый угол тангажа (для регулирования высоты)
 
  // Отправляем команды на симулятор
  // БВС будет пытаться выдерживать заданные углы крена и тангажа
  return _ans;
}
float roll_err_last = 0;
float Task_solution::GammaReg(float Psi, float Psi_dir)
{
  float Kpsi = 1.6;
  float roll_err = AngDefines(Psi - Psi_dir);
  float gamma_cmd = 3.8 * roll_err + 45 * (roll_err - roll_err_last);
  roll_err_last = roll_err;
  gamma_cmd = constrain(gamma_cmd, -20, 20);
  return gamma_cmd;
}
float Task_solution::HeightReg(float Yg, float Vy, float Hz)
{
  float K_H = 0.72;
  float K_vy_1 = 7.7591;
  float K_vy_2 = 9.8941;
  float kh1 = constrain(K_H * (Hz - Yg), -7.5, 7.5);
  float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20);
  return _Pitch_direct;
}
float Task_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, float Psi)
{
  float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt);
  Psi_cmd *= 57.3;
  return GammaReg(Psi, Psi_cmd);
}
float Task_solution::PointsFlyGam(const size_t& a_Point_index, float _Xt, float _Yt, float Psi)
{
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  if (userControlEnabled)
  {
    Xg_cmd = now_VPP.North;
    Zg_cmd = now_VPP.East;
  }
  return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi);
}
float Task_solution::PointsFlyTan(float H, float Vy, const size_t& a_Point_index)
{
  float H_z = _PointsArray[_Point_Index].Height;
  if (userControlEnabled)
  {
    H_z = now_VPP.Height;
  }
  return HeightReg(H, Vy, H_z);
}
size_t Task_solution::GetNowPointIndex(float X, float Z, float H)
{
  size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]);
  float Xg_cmd = _PointsArray[_Point_Index].North;
  float Zg_cmd = _PointsArray[_Point_Index].East;
  float Yg_cmd = _PointsArray[_Point_Index].Height;
  if (userControlEnabled)
  {
    Xg_cmd = now_VPP.North;
    Zg_cmd = now_VPP.East;
    Yg_cmd = now_VPP.Height;
  }
  float delX = Xg_cmd - X;
  float delY = Zg_cmd - Z;
  float delZ = Yg_cmd - H;
  float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2));
  if (Size < 7.0)
  {
    if (userControlEnabled)
      userControlEnabled = false;
    else
      _Point_Index++;
  }
  if (_Point_Index >= max_index)
  {
    _Point_Index = 0;
  }
  return _Point_Index;
}