Глава 4. Python API

4.1 Установка Python на Windows

• На сайте https://www.python.org/ скачайте последнюю версию Python, нажав на кнопку “Downloads”

Рисунок 4.1. Скачивание Python с официального сайта

• Запустите установщик, поставьте галочку «Add python.exe to PATH».

Рисунок 4.2. Запуск установщика Python

• Отключите ограничение длины пути

Рисунок 4.3. Отключение ограничения длины пути после установки

Для более удобного программирования можно установить различные интегрированные среды разработки для различных программ (Visual Studio Code, PyCharm и т.д).

Чтобы установить Visual Studio Code:

• Перейдите на сайт https://code.visualstudio.com/ и нажмите «Downloads for Windows»;

Рисунок 4.4. Скачивание Visual Studio Code с официального сайта

• Запустите установщик;

Рисунок 4.5. Выбор дополнительных задач при установке VS Code

После установки запустите приложение, зайдите в расширения, введите Python и установите его;

Рисунок 4.6. Установка расширения Python в VS Code

• После установки зайдите в проводник и создайте новый Python файл, после чего можно начинать программировать.

Рисунок 4.7. Стартовый экран VS Code

4.2 Установка Python на Linux

1. Для начала необходимо обновить список пакетов. Для этого откройте терминал, напишите команду и введите пароль:

sudo apt update

2. В большинстве случаев Python уже установлен в Linux. Чтобы установить последнюю версию Python, выполните:

sudo apt update

3. Установите pip:

pip – это пакетный менеджер для Python. Установите его с помощью команды:

sudo apt install python3-pip

Установите venv

Проверьте версию python

python3 --version

Затем в соответствии с версией установите venv командой

sudo apt install python3.<версия>-venv

Можно также установить Visual Studio Code.

• Для этого необходимо перейти к списку установленных приложений (находится в левом нижнем углу) и найти Ubuntu Software.

Рисунок 4.8. Переход в Ubuntu Software

• Теперь необходимо найти VS Code. Нажмите кнопку «Поиск» и введите «vscode» в строке поиска. Нажмите «код» в списке, чтобы открыть страницу приложения.

Рисунок 4.9. Страница поиска приложения VS Code

• Нажмите кнопку «Установить» и при появлении запроса введите пароль, чтобы начать установку.

Рисунок 4.10. Установка VS Code в Ubuntu Software

• Теперь Visual Studio Code должен появиться в списке ваших установленных приложений. Запустите его, зайдите в расширения, найдите Python и установите его.

Рисунок 4.11. Установка расширения Python в VS Code

• После установки зайдите в проводник и создайте новый Python файл, после чего можно начинать программировать.

Рисунок 4.12. Переход для создания нового Python файла

4.3 Установка Python API и библиотек

Для получения API необходимо перейти по ссылке и скачать пакет “drone_control_api”:

https://github.com/AppliedRobotics/ARA-EDU/tree/main/drone-control-api

Далее необходимо установить библиотеки. Для этого в соответствии с вашей операционной системой необходимо прописать в терминале среды программирования следующие команды:

Для Ubuntu, выполните:

python3 -m pip install -e ~/полный_путь_до_пакета

Рисунок 4.13. Установка API для Linux

Для Windows, выполните:

python -m pip install -e полный_путь_до_пакета

Рисунок 4.14. Установка API в терминале VS Code

После чего у вас установятся библиотека Drone и необходимые для нее зависимости, а именно:

• websockets

• opencv-python

• numpy

Эти зависимости автоматически устанавливаются вместе с пакетом. Однако, вы также можете установить их вручную, используя файл “requirements.txt”.

Для этого в терминале выполните:

Для Windows:

pip install -r requirements.txt

Для Ubuntu:

pip3 install -r requirements.txt

4.4 API для взаимодействия с дроном

API (Application Programming Interface) — это интерфейс программирования приложений. Он предоставляет набор методов и правил, с помощью которых одна программа может взаимодействовать с другой. API позволяет разработчикам использовать функциональность, которую предоставляет другая программа, библиотека, сервис или устройство, без необходимости вникать в её внутреннюю реализацию.

Принцип работы API:

1. Запрос от клиента. Программа (клиент) отправляет запрос к API, указывая, какую информацию или действие она хочет получить/выполнить.

2. Обработка запроса сервером/системой. Сторона, предоставляющая API, обрабатывает запрос. Она проверяет данные, производит вычисления или взаимодействует с другими системами.

3. Ответ от API. После обработки запроса API возвращает ответ.

Рассмотрим функционал API для взаимодействия с дроном на языке программирования Python:

connect (ip, port) — Вызов метода connect устанавливает соединение с дроном.ip – ip адрес через который устанавливается соединение, в формате str. Рекомендуется использовать имя хоста, а именно trackingcam3.local.port – порт через который устанавливается соединение, в формате str, порт – 1233.Возвращает True или False в зависимости от того удалось ли установить соединение или нет.
takeoff () — Взлет дрона на на высоту 1 метр.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
boarding () — Посадка дрона.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setZeroOdomOpticflow () — Выставляет точку в пространстве, относительно которой будет считаться одометрия (по умолчанию это точка взлета).Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
getOdomOpticflow() — Получение данных с датчика Opticflow (расстояние, на которое переместился дрон по координатам Xx и Yy относительно взлета или с точки, которая была установлена с помощью метода setZeroOdomOpticflow() ).Возвращает список из 3 значений в формате:[odom_x, odom_y, odom_z].Значение odom приходит в формате float и измеряется в метрах.
getLidar() — Получение данных с лидара.Возвращает список из 360 значений [dist_1, dist_2, dist_3, …], где каждое значение, соответствует расстоянию на соответствующем угле. В случае если лидар установлен!Значение dist приходит в формате float и измеряется в метрах.
getRPY() — Получение углов Roll, Pitch, Yaw (эти углы определяют положение дрона в пространстве, то есть определяет на какой угол и по какой оси дрон был повернут относительно начальной точки).Возвращает список из 3 значений в формате: [roll, pitch, yaw].Значение углов приходит в формате float и измеряется в радианах.
getHeightBarometer() — Получение высоты дрона с помощью Барометра.Возвращает значение высоты в формате float.
getHeightRange() — Получение высоты дрона с помощью лазерного дальномера.Возвращает значение высоты в формате float.
getLight() — Получение освещенности, с помощью датчика света.Возвращает значение высоты в формате float.
getUltrasonic() — Получение расстояния до препятствия с помощью ультразвукового датчика расстояния.Возвращает список в формате: [{"id": 0, "value": value_0}, {"id": 1, "value": value_1}, {"id": 2, "value": value_2}, {"id": 3, "value": value_3}]Значение value приходит в формате float и измеряется в сантиметрах.
getBlobs() — Получение параметров цветовых пятен.Возвращает список в формате:[{"id": "0", "center": {"x": center_x0, "y": center_y0}, "size": {"x": size_x0, "y": size_y0}}, {"id": "1", "center": {"x": center_x1, "y": center_y1}, "size": {"x": size_x1, "y": size_y1}}, {"id": "2", "center": {"x": center_x2, "y": center_y2}, "size": {"x": size_x2, "y": size_y2}}, {"id": "3", "center": {"x": center_x3, "y": center_y3}, "size": {"x": size_x3, "y": size_y3}}]
getImage() — Получение изображения с камеры.Возвращает изображение с камеры в формате numpy массива, совместимого с opencv.
getUtilsData() — Получение информации о параметрах дрона.Возвращает список в формате:[['ARMING_DISABLED_NAVIGATION_UNSAFE'], ['ACC', 'BARO', 'RANGEFINDER', 'OPFLOW', 'TEMP'], ['ANGLE_MODE', 'NAV_ALTHOLD_MODE', 'NAV_FW_AUTOLAND'], 49, [1500, 1500, 1500, 885, 1675, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500], [0.0, 0.0, 42949672.0], [-0.008726646259971651, -0.03665191262740524, -2.35619 44901923453], False, True]
getArucos() — Получение положения и ориентации Aruco маркеров относительно камеры дрона и ID маркеров.Если маркер был обнаружен, то возвращаются координаты aruco-маркеров относительно системы координат камеры в формате: [{'id': aruco_id, 'pose': {'position': {'x': aruco_pos_x, 'y': aruco_pos_y, 'z': aruco_pos_z}, 'orientation': {'x': aruco_orient_x, 'y': aruco_orient_y, 'z': aruco_orient_z}}}].Если маркеры не обнаружены, то возвращается ответ “no markers”.
getCameraPoseAruco() — Получение позиции и ориентации камеры дрона относительно ArUco маркеров и ID маркеров.Возвращаются координаты камеры относительно систем координат ArUco-маркеров в формате:[{'id': aruco_id, 'pose': {'position': {'x': camera_pos_x, 'y': camera_pos_y, 'z': camera_pos_z}, 'orientation': {'x': camera_orient_x, 'y': camera_orient_y, 'z': camera_orient_z}}}]Если маркеры не обнаружены, то выдается ответ “no markers”.
getArm() — Получение информации о состоянии дрона.Возвращает пустой список [], если дрон еще не был переведен в режим Arm после включения. Возвращает список ['ARMED', 'WAS_EVER_ARMED'], если в момент вызова метода, дрон переведен в режим Arm.Возвращает список ['WAS_EVER_ARMED'], если дрон не переведен в режим Arm, но после своего включения уже был когда-либо переведен в режим Arm.
setYaw(yaw) — Установка угла (yaw), на который дрону необходимо повернуться (угол задается относительно стартовой позиции дрона, а не от текущей. Если аргумент функции положительный, то дрон будет вращаться по часовой стрелке).yaw - целевой угол в радианах в формате float.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setVelXY(vel_x, vel_y) — Установка скорости по осям X и Y соответственно.vel_x - целевая скорость по оси X в формате floatvel_y - целевая скорость по оси Y в формате floatВозвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setVelXYYaw(vel_x, vel_y, vel_yaw) — Установка скорости по осям X и Y соответственно.vel_x - целевая скорость по оси X в формате floatvel_y - целевая скорость по оси Y в формате floatvel_yaw - целевая скорость по yaw в формате float (+ вращение по часовой, - вращение против часовой стрелки)Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
gotoXYdrone(drone_x, drone_y) — Установка целевой точки для дрона относительно его текущего положения.drone_x - целевое значение положение относительно текущей координаты дрона по оси X в формате floatdrone_y - целевое значение положение относительно текущей координаты дрона по оси Y в формате floatВозвращает “OK”, если метод был вызван, и был получен ответ сервиса.
gotoXYodom(odom_x, odom_y) — Установка целевой точки для дрона относительно его 0 по одометрии.odom_x - целевое значение положение относительно начальной точки одометрии по оси X в формате floatodom_y - целевое значение положение относительно начальной точки одометрии по оси Y в формате floatВозвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setHeight(target_height) — Установка высоты дрона в метрах относительно земли.target_height - целевая высота для дрона в формате float.Высота задается только в границах от [0.05, 2.0] в метрах, иначе сервис выдаст ошибку и не выполнит изменение высоты.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setMagnet(val) — Управление магнитом.val - состояние магнита, принимает True или False (включение и выключение магнита соответственно)Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setDiod(r, g, b) — Управление светодиодом.r - компонента отвечающая за красный цвет;g - компонента отвечающая за зеленый цвет;b - компонента отвечающая за синий цвет;Каждая компонента принимает значение от 0 до 255 в формате int.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
setBeeper(power, freq) — Управление пьезопищалкой.power - мощность звука.freq - частота звука.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.
disconnect() — Метод, закрывающий соединение с сервером.Возвращает “OK”, если метод был вызван, и был получен ответ сервиса.

4.5 Ознакомление с методами API для взаимодействия с дроном

Рассмотрим программу example.py для взаимодействия с дроном. Данная программа позволит ознакомиться с методами API для взаимодействия с дроном.

from drone_control_api import Drone

import datetime

import cv2

ip = "10.42.0.1"

port = "1233"

last_time = None

def OnMes0(mes):

    global last_time

    current_time = datetime.datetime.now()

    if last_time is not None:

        delta_time = (current_time - last_time).total_seconds()

    else:

        delta_time = None

    last_time = current_time

    if delta_time is not None:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"

    else:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"

def OnMesImage(img):

    print(f"img = {img}\n")

    cv2.imwrite('img', img)

client = Drone()

client.AddOnMessangeUtils(OnMes0)

client.AddOnMessangeImage(OnMesImage)

print("connected?", client.connect(ip, port), "\n")

print("takeoff?", client.takeoff(), "\n")

print("boarding?", client.boarding(), "\n")

print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")

print("getOdomOpticflow?", client.getOdomOpticflow(), "\n")

print("getLidar?", client.getLidar(), "\n")

print("getRPY?", client.getRPY(), "\n")

print("getHeightBarometer?", client.getHeightBarometer(), "\n")

print("getHeightRange?", client.getHeightRange(), "\n")

print("getArm?", client.getArm(), "\n")

print("setYaw?", client.setYaw(0), "\n")

print("setVelXY?", client.setVelXY(0, 0), "\n")

print("setVelXYYaw?", client.setVelXYYaw(0, 0, 0), "\n")

print("gotoXYdrone?", client.gotoXYdrone(0, 0), "\n")

print("gotoXYodom?", client.gotoXYodom(0, 0), "\n")

print("setHeight?", client.setHeight(0), "\n")

print("getArucos?", client.getArucos(), "\n")

print("getCameraPoseAruco?", client.getCameraPoseAruco(), "\n")

print("setMagnet?", client.setMagnet(False), "\n")

print("getBlobs?", client.getBlobs(), "\n")

print(f"getImage = {client.getImage()}")

imgg = client.getImage()

cv2.imshow("imgg", imgg)

cv2.waitKey(0)

utils = client.getUtilsData()

print(utils)

print("disconnected?", client.disconnect(), "\n")

frome drone_control_api import Drone — импортирует библиотеку Drone, которая предоставляет API для работы с дроном. Она содержит методы для управления дроном и получения данных с его датчиков.

import datetime — импортирует библиотеку для работы с датой и временем.

import cv2 — импортирует библиотеку OpenCV, которая широко используется для обработки изображений и компьютерного зрения. OpenCV предоставляет множество функций для работы с изображениями и видео.

В строках 5 и 6 указываются параметры подключения к дрону. IP-адрес, к которому подключается клиент. В данном случае 10.42.0.1 — это IP-адрес TrackingCam v3, и порт, через который идет связь. В данном случае 1233.

Функция OnMes0 — служит для обработки сообщений. Она принимает сообщение от дрона (mes), после чего фиксирует момент получения сообщения:

current_time = datetime.datetime.now()

Если уже было получено сообщение, то вычисляет интервал времени в секундах:

delta_time = (current_time - last_time).total_seconds()

После чего формирует текст для записи логов и сохраняет их в файл messages_log.txt.

В строке 28 создается объект клиента client, который используется для взаимодействия с дроном через API:

client = Drone()

Далее вызывается метод из библиотеки Drone, который регистрирует функцию-обработчик сообщений: client.AddOnMessangeUtils(OnMes0)

Со строки 33 перечислены вызовы различных методов API.

4.6 ПИД-регулятор

Для удобства использования в API интегрирован модуль ПИД регулирования, который можно импортировать строкой:

from drone_control_api.pid import PID

Ниже представлен его исходный код:

class PID:

    def __init__(self, Kp, Ti, Td):

        self.Kp = Kp

        self.Td = Td

        self.Ti = Ti

        self.curr_error = 0

        self.prev_error = 0

        self.sum_error = 0

        self.prev_error_deriv = 0

        self.curr_error_deriv = 0

        self.control = 0

    def update_control(self, current_error, reset_prev=False):

        self.prev_error = self.curr_error

        self.curr_error = current_error

        # Calculating the integral error

        self.sum_error = self.sum_error + self.curr_error

        # Calculating the derivative error

        self.curr_error_deriv = self.curr_error - self.prev_error

        # Calculating the PID Control

        self.control = self.Kp * self.curr_error + self.Ti * self.sum_error + self.Td * self.curr_error_deriv

    def get_control(self):

        return self.control

Данный модуль реализует классический PID регулятор с заданными коэффициентами усиления. Его использование можно увидеть в примерах ниже.

4.7 Алгоритм автономного запуска БПЛА при использовании Python API

1. Установите дрон в защитном кубе;

2. Включите пульт управления;

3. Подключите АКБ так, чтобы дрон не повернулся. Это важно, так как в момент включения инициализируется система координат дрона и направление осей;

4. Переведите квадрокоптер в режим ARM, моторы активируются;

5. Включите режимы AltHold и PosHold;

6. Переключите тумблер, закрепленный за 8 каналом в нижнее крайнее положение, это переключит квадрокоптер на управление с TrackingCam3 и введет его в режим автономного полета, управление с пульта будет недоступно.

7. Откройте папку с API в установленном IDE. В случае, если вы работаете в VS Code нажмите File Open Folder и выберите папку drone-control-api:

Рисунок 4.15. Открытие папки с API в VS Code

8. Перед запуском программы проверьте ip и порт подключения. О том, как узнать IP камеры и даже изменить его, вы можете прочитать в подразделе 1.3.3.

WARNING

При неправильном ip и port метод client.connect(ip, port) вернет False, и программа не будет выполнена.

9. Запустите виртуальное окружение, если это необходимо.

При запуске кода (этап 10) виртуальное окружение активируется автоматически. Появляется папка .venv и префикс с окружением.

[!warnibg] Если окружение активируется, пропустите этот этап, однако если виртуальное окружение не создается и не активируется автоматически, и код при этом не запускается, выполните следующие шаги:

• Создайте виртуальное окружение (этот шаг нужно проделать только один раз)

• Если работа производится на Windows, введите команду:

python -m venv .venv

• Если работа производится на Linux, введите команду

python3 -m venv .venv

• Когда команда будет выполнена, вы увидите папку .venv

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

Если работа производится на Windows, введите команду:

.\.venv\Scripts\activate

Если работа производится на Linux, введите команду

source .venv/bin/activate

Если все было сделано правильно, то в командной строке появится префикс (.venv) с окружением:

Для Windows:

Для Ubuntu:

10. Запустите выполнение кода. Существует два способа запуска выполнения скрипта:

В терминале введите команду:

Для Windows:

python <путь до файла со скриптом>

Например, python '.\examples\Кейсы Python\Взлет и поворот\Takeoff_and_rotate.py';

Для Linux:

python3 <путь до файла со скриптом>

Например, python3 examples/Кейсы\ Python/Взлет\ и\ поворот/Takeoff_and_rotate.py. Когда в названии папок есть пробелы, в линукс ставится обратный слеш перед ними;

Нажмите Enter, дрон начнет выполнение кода.

• Используйте интерфейс VS Code:

Рисунок 4.16. Запуск программы через интерфейс VS Code

11. После выполнения скрипта верните дрон в ручное управление, переведя тумблер 8 канала в верхнее крайнее положение. Управление через пульт снова станет доступно. Удерживайте на пульте уровень газа посередине, чтобы дрон при переключении не упал.

12. После завершения посадки переведите дрон в режим DISARM на пульте управления;

13. Отключите АКБ;

14. Выключите пульт.

4.8 Примеры программ с использованием API

В репозитории представлены примеры использования Python API, которые можно найти в папке examples/Кейсы Python. Системы координат в Python API такие же, как описаны в примерах Blockly.

Перечень примеров:

Takeoff_and_landing.py – пример взлета и посадки

Takeoff_and_rotate.py – пример взлета, поворота и посадки дрона

Takeoff_and_fly_forward.py – пример взлета, пролета вперед и посадки дрона

Square.py – пример полета по квадрату с помощью одометрии

ObstacleDetector.py – пример обнаружения препятствий при помощи ультразвукового дальномера с сигнализацией трехцветным светодиодом о состоянии дрона. Используется ПИД регулятор.

Wall_Follower_R_reg.py – пример полета вдоль стены при помощи ультразвукового дальномера c релейным регулятором.

Wall_Follower_PID_reg.py – пример полета вдоль стены при помощи ультразвукового дальномера c ПИД-регулятором.

TrackingAruco_P_Reg.py – пример слежения за ArUco-маркером при помощи П-регулятора.

ArucoBoarding.py – пример выполнения посадки на аруко маркер при помощи ПИД регулирования. Для теста этого примера необходимо направить камеру вниз, чтобы видеть маркер расположенный на полу.

TwoArucos.py – пример полета по двум ArUco маркерам, наклеенным на стенах полетной зоны. После достижения каждого из маркеров дрон подает сигнал светодиодом.

BlobAvoid_PID_Reg.py – пример обнаружения и облета цветового блоба.

4.8.1 Простые программы на Python

В программах ниже используется базовый Ip камеры 10.42.0.1. Если вы поменяли IP или камера подключена к сети рабочего пространства, то прописывайте IP-адрес своей камеры!

Взлет и посадка (Takeoff_and_landing.py)

Дрон устанавливает соединение. Взлет, задержка 10 секунд, чтобы дрон успел выполнить взлет, посадка, разрыв соединения.

from drone_control_api import Drone

import time

ip = "10.42.0.1"

port = "1233"

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(10)

print("boarding?", client.boarding(), "\n")

print("disconnected?", client.disconnect(), "\n")

Взлет и поворот (Takeoff_and_rotate.py)

Дрон устанавливает соединение. Взлет, задержка на 10 секунд, поворот по часовой стрелке относительно начального положения, в котором был включен дрон. Угол поворота указывается в радианах. Последняя команда – разрыв соединения.

from drone_control_api import Drone

import time

ip = "10.42.0.1"

port = "1233"

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(10)

print("setYaw?", client.setYaw(1.57), "\n") #поворот по часовой стрелке относительно положения, в котором дрон был включен (угол задается в радианах)

print("boarding?", client.boarding(), "\n")

print("disconnected?", client.disconnect(), "\n")

Взлет и пролет вперед (Takeoff_and_fly_forward.py)

Дрон устанавливает соединение. Взлет, задержка 10 секунд. Командой client.setZeroOdomOpticflow() ‒ обнуляется одометрия. Далее задается полет вперед по одометрии на 1,5 метра. Затем посадка, разрыв соединения.

from drone_control_api import Drone

import time

ip = "10.42.0.1"

port = "1233"

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(10)

print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")

print("gotoXYdrone?", client.gotoXYdrone(1.5, 0), "\n") #полет вперед по одометрии на 1.5м

#print("gotoXYodom?", client.gotoXYodom(1.5, 0), "\n") #полет вперед относительно текущей позиции дрона

print("boarding?", client.boarding(), "\n")

print("disconnected?", client.disconnect(), "\n")

Пролет по квадрату (Square.py)

Дрон устанавливает соединение. Взлет, задержка 10 секунд, обнуление одометрии.

Далее задается полет по одометрии на 1 метр: client.gotoXYodom (1, 0). Метод повторяется 4 раза. Значения координат изменяются в соответствии с необходимым положением – координатой квадрата. Началом координат считается положение, в котором обнулилась одометрия. Далее идет посадка, разрыв соединения.

Строки 22-25 закомментированы и показывают пример полета по квадрату, используя метод gotoXYdrone, где система координат начинается от текущего положения дрона.

from drone_control_api import Drone

import time

ip = "10.42.0.1"

port = "1233"

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(10)

print("setZeroOdomOpticflow?", client.setZeroOdomOpticflow(), "\n")

print("gotoXYodom?", client.gotoXYodom(1, 0), "\n")

print("gotoXYodom?", client.gotoXYodom(1, 1), "\n")

print("gotoXYodom?", client.gotoXYodom(0, 1), "\n")

print("gotoXYodom?", client.gotoXYodom(0, 0), "\n")

# print("gotoXYdrone?", client.gotoXYdrone(1, 0), "\n")

# print("gotoXYdrone?", client.gotoXYdrone(0, 1), "\n")

# print("gotoXYdrone?", client.gotoXYdrone(-1, 0), "\n")

# print("gotoXYdrone?", client.gotoXYdrone(0, -1), "\n")

print("boarding?", client.boarding(), "\n")

print("disconnected?", client.disconnect(), "\n")

Обнаружение препятствия (ObstacleDetector.py)

Для обнаружения препятствия используется ультразвуковой датчик расстояния, с помощью него мы будем получать расстояние от дрона до стены. В этом примере также появляется управление светодиодом и пищалкой.

Программа предназначена для автоматического полета дрона вперед, пока не будет обнаружено препятствие, после этого полет вперед прекращается, загорается красный цвет светодиода и подается звуковой сигнал.

import time

from drone_control_api import Drone

from drone_control_api import pid

ip = "192.168.1.197"

port = "1233"

# Функция обнаружения препятствий

def obstacle_detector():

    distance = 0.7

    while True:

        distance_to_wall = client.getUltrasonic()[0]['value']

        distance_to_wall /= 100

        print('dist to wall', distance_to_wall)

# Если расстояние до препятствия больше целевого, продолжаем полет вперед

        if distance_to_wall > distance:

            client.setDiod(0, 255, 0)

            client.setVelXY(0.9, 0)

            client.setBeeper(0, 0)

# Если расстояние меньше или равно целевому, сбрасываем скорости и выходим из цикла

        else:

            client.setDiod(255, 0, 0.0)

            client.setBeeper(50, 1000)

            client.setVelXY(0, 0)

            break

client = Drone()

print("connected?", client.connect(ip, port), "\n")

client.setDiod(0,0,0)

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(5)

client.setHeight(0.5)

obstacle_detector()

time.sleep(3)

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("boarding?", client.boarding(), "\n")

time.sleep(3)

client.setDiod(0.0, 0.0, 0.0)

client.setBeeper(0.0, 0.0)

В функции obstacle_detector cоздается переменная distance, равная 0.7 метров. Все, что ближе этого расстояния считается препятствием. Расстояние, измеренное ультразвуковым датчиком, выводится в консоль для удобства. Дрон выполняет взлет на 1 метр, а затем опускается на высоту 0.5 метров client.setHeight (0.5) и летит вперед с постоянной скоростью client.setVelXY(0.9, 0).

Если расстояние между дроном и препятствием больше distance , то светодиод горит зеленым цветом и дрон продолжает лететь с заданной скоростью. Если же препятствие обнаружено, то загорается красный светодиод, подается звуковой сигнал и дрон останавливается, цикл завершается и выполняется посадка.

Полет вдоль стены с использованием релейного регулятора(Wall_Follower_R_reg.py)

В данном примере мы также будем использовать ультразвуковой датчик расстояния, с помощью которого можно получить расстояние от дрона до стены, а также светодиод для сигнализации.

from drone_control_api import Drone

import time

ip = "10.42.0.1"

port = "1233"

def wall_follow():

    distance = 0.5

    while True:

        distance_to_wall = client.getUltrasonic()[0]['value']

        distance_to_wall /= 100

        print('dist to wall', distance_to_wall)

        pitch_error = distance_to_wall - distance

        print('pitch_error', pitch_error)

        if distance_to_wall > distance:

            client.setDiod(0, 255, 0)

        else:

            client.setDiod(255, 0, 0)

        r_regulator(pitch_error)

def r_regulator(pitch_error):

    max_speed_roll = -0.65

    pitch_speed = sign(pitch_error) * 0.65

    client.setVelXY(pitch_speed, max_speed_roll)

def sign(value):

    if value >= 0:

        value = 1

    else:

        value = -1

    return value

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(5)

wall_follow()

Переменная distance, равная 0.5 метров, является целевым расстоянием до стены. В функции wall_follow() происходит расчет ошибки: в бесконечном цикле расстояние, измеренное ультразвуковым датчиком, преобразуется из сантиметров в метры и выводится в консоль для удобства. В переменной pitch_error рассчитывается ошибка – разница между истинным и целевым расстоянием. Если расстояние между дроном и стеной больше целевого, загорается зеленый светодиод, иначе – красный. А затем в цикле вызывается функция r_regulator с переданной в нее переменной pitch_error.

В функции r_regulator задаются и рассчитываются скорости для движения дрона (pitch_speed и max_speed_roll). В функции sign определяется направление движения, куда в качестве value передается переменная pitch_error. Если ошибка больше или равна 0 (то есть дрон не достиг целевого расстояния или имеет нужное положение), то направление движения дрона будет положительным, и он полетит вперед. Если же pitch_error меньше 0 (то есть дрон находится ближе к стене, чем это нужно), то функция sign вернет значение -1, и дрон полетит назад с заданной скоростью.

Далее дрон выполняет взлет на 1 метр, опускается на высоту 0.5 метров client.setHeight (0.5) и выполняется функция wall_follow. Дрон будет лететь влево и выравниваться относительно стены и целевого расстояния до нее.

Полет вдоль стены с использованием ПИД-регулятора(Wall_Follower_PID_reg.py)

Для полета вдоль стены также используется ультразвуковой датчик расстояния.

import time

from drone_control_api import Drone

from drone_control_api.pid import PID

import datetime

ip = "10.42.0.1"

port = "1233"

def WallFollow():

    pid_pitch= PID(2.5, 0.0, 0.5)

    distance = 0.5

    pitch_error = 0.0

    response_distance = 1.0

    while True:

        try:

            distance_to_wall = client.getUltrasonic()[0]['value']

            distance_to_wall /= 100

            print('distance to wall', distance_to_wall)

            if distance_to_wall < response_distance :

                pitch_error = distance_to_wall - distance

                print('pitch_error', pitch_error)

                DefaultRegulation(pitch_error, pid_pitch)

            else:

                client.setVelXY(1.0, 0)

        except KeyboardInterrupt:

            print("KeyboardInterrupt detected, landing the drone...")

            break

def DefaultRegulation(pitch_error, pid_pitch: PID):

    accuracy = 0.1

    max_pid = 1.0

    max_speed_roll = 0.9

    if abs(pitch_error) > accuracy:

        pid_pitch.update_control(pitch_error)

        PID_PITCH = pid_pitch.get_control()

        PID_PITCH = constrain (PID_PITCH, max_pid)

    else:

        PID_PITCH = 0

    print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_PITCH }")

    client.setVelXY(PID_PITCH, max_speed_roll)

def constrain(value, threshold):

    if value > threshold:

        value = threshold

    if value < -threshold:

        value = -threshold

    return value

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("takeoff?", client.takeoff(), "\n")

time.sleep(4)

client.setHeight(0.5)

time.sleep(5)

print("Wall Follow: ", WallFollow(), "\n")

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("boarding?", client.boarding(), "\n")

Программа запускается в бесконечном цикле, где дрон постоянно измеряет расстояние до стены с помощью ультразвукового датчика. Это расстояние преобразуется из сантиметров в метры. Как только дрон получает данные о расстоянии до стены, он проверяет, насколько близко он к ней. Если расстояние меньше заданного response_distance (1.0 метра), это сигнализирует о том, что дрон слишком близко к стене, и ему нужно начинать лететь в сторону.

Здесь вступает в действие функция DefaultRegulation. Функция принимает ошибку по pitch, которая определяется как разница между текущим расстоянием до стены и заданным значением distance (0.5 метра). Если ошибка превышает определенный порог точности accuracy (0.1 метр), PID-регулятор обновляет свои управляющие параметры и вычисляет новую команду для дрона. Помимо этого, при вызове функции DefaultRegulation дрону присваивается скорость для полета вдоль стены max_speed_roll.

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

Функция constrain следит за тем, чтобы значения управляющих параметров не выходили за пределы допустимых значений, обеспечивая безопасность и стабильность управления.

Программа также обрабатывает возможные прерывания, например, если пользователь решит остановить выполнение. В этом случае дрон выполняет безопасную посадку.

4.8.2 Сложные программы на Python

Слежение за ArUco маркером при помощи П-регулятора (TrackingAruco_P_Reg.py)

В данном примере реализуется слежение и выравнивание относительно маркера, а также используется трехцветный светодиод для сигнализации о распознавании маркера.

Перед выполнением данного примера проверьте раздел «Настройки» Графического интерфейса. Включите «Обнаружение маркеров ArUco» и выполните настройку маркеров: выберите словарь и размер маркера.

from drone_control_api import Drone

from drone_control_api.pid import PID

import time

ip = "10.42.0.1"

port = "1233"

# Функция поиска маркера

def search_aruco (yaw_vel):

    client.setDiod(255, 0, 0)

    client.setVelXYYaw(0, 0, yaw_vel)

    aruco_state = True

    while aruco_state:

        errors = client.getArucos()

        if errors:

            aruco_state = False

            client.setDiod(0, 255, 0)

            client.setVelXYYaw(0, 0, 0)

# Функция расчета ошибок

def tracking_aruco():

    pitch_error = 0

    roll_error = 0

    yaw_error = 0

    distance = 1

    accuracy_height = 0.15

    while True:

        errors = client.getArucos()

        if errors:

            client.setDiod(0, 255, 0)

            distance_to_marker = errors [0]['pose']['position']['z']

            pitch_error = distance_to_marker - distance

            roll_error = errors[0]['pose']['position']['x']

            height_error = errors[0]['pose']['position']['y']

            yaw_error = -1 * errors[0]['pose']['orientation']['z']

            print(f"Дистанция до маркера: {distance_to_marker}")

            if abs(height_error) > accuracy_height:

                height_regulation(height_error)

            aruco_regulation (pitch_error, roll_error, yaw_error)

        else:

            client.setDiod(255, 0, 0)

            aruco_regulation (pitch_error, roll_error, 0)

            print (f"Маркер потерян! Поиск:")

            print (f"pitch_error = {pitch_error}, roll_error = {roll_error}")

# Выравнивание по высоте

def height_regulation(height_error):

    current_height = client.getHeightRange()

    new_height = current_height - height_error

    print(f"Новая высота {new_height}")

    client.setHeight(new_height)

# Выравнивание относительно маркера

def aruco_regulation(pitch_error, roll_error, yaw_error):

    PID_pitch = pitch_error * 1.5

    PID_pitch = constrain (PID_pitch, 1)

    PID_roll = roll_error * 1.5

    PID_roll = constrain (PID_roll, 1)

    PID_yaw = constrain (yaw_error, 0.3)

    print(f"PID_pitch={PID_pitch}, PID_roll={PID_roll}, PID_yaw={PID_yaw}")

    client.setVelXYYaw(PID_pitch, PID_roll, PID_yaw)

# Функция ограничения величины

def constrain (value, threshold):

    if value > threshold:

        value = threshold

    if value < -threshold:

        value = -threshold

    return value

client = Drone()

# Подключаемся

print("connected?", client.connect(ip, port), "\n")

# Сбрасываем скорости

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

# Взлет

print("takeoff?", client.takeoff(), "\n")

time.sleep(7)

# Ищем маркер

search_aruco(0.5)

# Выравнивание относительно маркера

tracking_aruco()

В функции search_aruco происходит вращение дрона вокруг своей оси со скоростью yaw_vel, которая передается с вызовом этой функции. Как только маркер обнаружен, светодиод загорается зеленым и сбрасываются скорости для остановки дрона.

В функции tracking_aruco происходит расчет ошибок, которые затем передаются в функции для выравнивания относительно маркера (height_ regulation и aruco_regulation). Здесь в переменную errors мы получаем положение маркера в системе координат камеры.

В ошибку по pitch записывается разница между истинным расстоянием до маркера и желаемым. Расстояние рассчитывается исходя из площади, поэтому важно ввести правильный размер маркера. В ошибку по roll – положение маркера относительно центра камеры по оси х, в ошибку по высоте (height_error) – положение маркера относительно центра камеры по оси y, в ошибку по yaw – отрицательный поворот маркера.

Переменная accurac_height хранит пороговое значение. Если ошибка по высоте будет больше него, дрон перейдет к функции height_regulation – выравниванию по высоте.

Функция aruco_regulation принимает ошибки по pitch, roll и yaw и рассчитывает скорости по этим осям. Затем полученные значения сравниваются с threshold в функции constrain для ограничения максимальной скорости и передаются в метод setVelXYYaw.

Дрон выполняет взлет на 1 метр, переходит к поиску маркера со скоростью вращения 0.5, как в данном примере и выполняет выравнивание относительно маркера.

Посадка на ArUco маркер(ArucoBoarding.py)

Посадка на ArUco маркер должна производиться следующим способом:

1. Камера, направленная вниз получает данные о положении ArUco маркера с помощью метода getArucos(**).**

2. С помощью этих данных производится выравнивание дрона относительно маркера.

3. После того, как дрон попадает в некоторую область, регламентирующую точность выравнивания, он снижает высоту.

4. При определенном значении высоты, полученной с помощью метода getHeightRange() дрон осуществляет посадку.

В функции ArucoRegulation происходят пид-регулировка входных значений ошибок по положению и установка необходимых скоростей для движения дрона к центру маркера;

Функция constrain нужна для установления максимальных значений скоростей с выхода пид-регулятора (в данном примере значения могут находиться в диапазоне от -1.1 до 1.1).

Пример кода для реализации посадки на ArUco маркер представлен ниже:

# Библиотеки для работы со временем

import time

from datetime import datetime

# Класс для работы с API дрона

from drone_control_api import Drone

# Импортируем класс PID

from drone_control_api.pid import PID

# Задаем IP и порт для подключения к дрону

ip = "10.42.0.1"

port = "1233"

# Переменная для хранения времени последнего сообщения

last_time = None

# Функция посадки дрона на аруко-маркер

def Aruco_boarding():

    # Время начала отсчёта

    time_begin = datetime.now()

    # Инициализируем переменную для текущего времени

    time_now = time_begin

    # Находим их разность

    delta_time = time_now - time_begin

    # Инициализируем переменную для текущей высоты

    current_height = client.getHeightRange()

    # Выводим текущую высоту

    print(f"Started Height is: {current_height}")

    # Инициализация PID-контроллеров для крена и тангажа

    pid_pitch = PID(0.7, 0.0, 0.1)

    pid_roll = PID(0.7, 0.0, 0.1)

    # Инициализируем переменные ошибок

    pitch_error = 0.0

    roll_error = 0.0

    # Устанавливаем время выполнения, по истечении которого дрон совершит посадку

    while delta_time.total_seconds() < 100:

        try:

            # Обновляем текущее время и рассчитываем время выполнения

            time_now = datetime.now()

            delta_time = time_now - time_begin

            # С помощью сервиса getArucos получаем координаты маркера относительно дрона — это и есть ошибки

            errors = client.getArucos()

            # Если камера не видит аруко-маркеров

            if errors == []:  # Потерян маркер

                print('I dont see Aruco')

                # Вызываем функцию регулировки

                ArucoRegulation(pitch_error, roll_error, pid_pitch, pid_roll)

                # Продолжаем выполнение цикла

                continue

            else:

                # Если камера видит аруко-маркеры, записываем в переменные ошибки

                roll_error = errors[0]['pose']['position']['x']

                pitch_error = errors[0]['pose']['position']['y']

                # Вызываем функцию регулировки

                ArucoRegulation(pitch_error, roll_error, pid_pitch, pid_roll)

                # Если ошибки меньше 30 по x и y - снижаем высоту

                if abs(pitch_error) < 0.3 and abs(roll_error) < 0.3:

                    current_height *= 0.75

                    print("Снижение")

                    # Посылаем в сервис setHeight новую высоту

                    client.setHeight(current_height)

                    print(f"Current Height is: {current_height}")

                if current_height < 0.4:

                    # Останавливаем дрон и завершаем посадку

                    client.setVelXY(0, 0)

                    print("Landing")

                    client.boarding()

                    break

        except KeyboardInterrupt:

            # Обрабатываем прерывание программы с клавиатуры

            print("KeyboardInterrupt detected, landing the drone...")

            break

# Функция регулировки положения дрона относительно аруко-маркера

def ArucoRegulation(pitch, roll, pid_x: PID, pid_y: PID):

    print(f"Adjusting position: pitch={pitch}, roll={roll}")

    # Обновление контроллеров по осям

    pid_x.update_control(pitch)

    pid_y.update_control(roll)

    PID_PITCH = -pid_x.get_control()

    PID_ROLL = pid_y.get_control()

    # Ограничение управляющих сигналов

    PID_PITCH = constrain(PID_PITCH, 1.1)

    PID_ROLL = constrain(PID_ROLL, 1.1)

    print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}")

    # Устанавливаем скорости для дрона

    client.setVelXY(PID_PITCH, PID_ROLL)

    return (PID_PITCH, PID_ROLL)

# Функция для ограничения значений

def constrain(value, threshold):

    if value > threshold:

        value = threshold

    if value < -threshold:

        value = -threshold

    return value

# Функция обработки входящих сообщений

def OnMes0(mes):

    global last_time

    current_time = datetime.now()

    if last_time is not None:

        delta_time = (current_time - last_time).total_seconds()

    else:

        delta_time = None

    last_time = current_time

    if delta_time is not None:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"

    else:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"

    # Запись сообщения в лог

    with open("messages_log.txt", "a") as file:

        file.write(log_message)

# Создаем объект клиента для взаимодействия с дроном

client = Drone()

# Добавляем обработчик сообщений

client.AddOnMessangeUtils(OnMes0)

# Подключаемся к дрону и выполняем команды

print("connected?", client.connect(ip, port), "\n")

print("VelCorrect", client.setVelXY(0, 0), "\n")

print("takeoff?", client.takeoff(), "\n")

print("Pose aruco: ", client.getArucos(), "\n")

time.sleep(8)

client.setHeight(1.5)

time.sleep(5)

print("getHeightRange?", client.getHeightRange(), "\n")

print("getOdomOpticflow?", client.getOdomOpticflow(), "\n")

print("Aruco Boarding: ", Aruco_boarding(), "\n")

print("boarding?", client.boarding(), "\n")

print("disconnected?", client.disconnect(), "\n")

Облет двух ArUco маркеров (TwoArucos.py)

В данном примере дрон взлетает, ищет первый ArUco маркер (функция SearchForArucos), после чего выравнивается относительно него (функция ArucoRegulation), мигает светодиодом, после чего отлетает (AvoidAruco), ищет следующий маркер (SearchForArucos), снова выравнивается (ArucoRegulation), сигнализирует об этом светодиодом и совершает посадку.

import time

from drone_control_api import Drone

from drone_control_api.pid import PID

import datetime

import math

ip = "10.42.0.1"

port = "1233"

last_time = None

def OnMes0(mes):

    global last_time

    current_time = datetime.datetime.now()

    if last_time is not None:

        delta_time = (current_time - last_time).total_seconds()

    else:

        delta_time = None

    last_time = current_time

    if delta_time is not None:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: {delta_time:.2f} seconds\n"

    else:

        log_message = f"Timestamp: {current_time}, Message: {mes}, Delta Time: N/A\n"

    with open("messages_log.txt", "a") as file:

        file.write(log_message)

#Функция поиска маркера

def SearchForArucos():

    client.setDiod(255.0, 0.0, 0.0)

    errors = []

    while len(errors) == 0:

        client.setVelXYYaw(0, 0, -0.5)

        errors = client.getArucos()

        print(errors)

    client.setVelXYYaw(0, 0, 0)

#Функция для отлёта от старого маркера

def AvoidAruco():

    client.setDiod(0.0, 0.0, 255.0)

    client.setVelXYYaw(0, 0, -0.5)

    time.sleep(5)

    client.setVelXYYaw(0, 0, 0)

# Выравнивание относительно маркера

def ArucoRegulation():

    client.setDiod(0.0, 255.0, 0.0)

    pid_pitch= PID(1.0, 0.002, 0.2)

    pid_roll = PID(1.0, 0.002, 0.2)

    pid_yaw = PID(0.6, 0.0, 0.5)

    distance = 1.0

    pitch_error = 0.0

    roll_error = 0.0

    height_error = 0.0

    yaw_error = 0.0

    last_height_regulation_time = 0

    last_yaw_regulation_time = 0

    while True:

        try:

            errors = client.getArucos()

            print(errors)

            if errors:

                roll_error = errors[0]['pose']['position']['x']

                distance_to_marker = errors[0]['pose']['position']['z']

                height_error = errors[0]['pose']['position']['y']

                yaw_error = errors[0]['pose']['orientation']['z']

                pitch_error = distance_to_marker - distance

                print(f"Дистанция до маркера: {distance_to_marker}")

                current_time = time.time()

                delta_time_height = current_time - last_height_regulation_time

                delta_time_yaw = current_time - last_yaw_regulation_time

                print(height_error)

                if abs(height_error) > 0.2 and delta_time_height > 5.0:

                    print("height correction")

                    last_height_regulation_time = time.time()

                DefaultRegulation(pitch_error, roll_error, pid_pitch, pid_roll, yaw_error, pid_yaw)

                previous_yaw = yaw_error

                if abs(height_error) < 0.1 and abs(pitch_error) < 0.15 and abs(roll_error) < 0.15:

                    print("Выравнивание произошло успешно")

                    sucess_flag = 1

                    break

            else:

                print("Dont see aruco")

                DefaultRegulation(pitch_error, roll_error, pid_pitch, pid_roll, 0, pid_yaw)

                previous_yaw = yaw_error

        except KeyboardInterrupt:

            print("KeyboardInterrupt detected, landing the drone...")

            break

def DefaultRegulation(pitch_error, roll_error, pid_p: PID, pid_r: PID, yaw_error, pid_yaw: PID):

    accuracy = 0.0

    max_pid = 0.8

    if abs(pitch_error) > accuracy:

        pid_p.update_control(pitch_error)

        PID_PITCH = pid_p.get_control()

        PID_PITCH = constrain(PID_PITCH, max_pid)

    else:

        PID_PITCH = 0

    if abs(roll_error) > accuracy:

        pid_r.update_control(roll_error)

        PID_ROLL = pid_r.get_control()

        PID_ROLL = constrain(PID_ROLL, max_pid)

    else:

        PID_ROLL = 0

    if abs(yaw_error) > 0.2:

        pid_yaw.update_control(yaw_error)

        PID_YAW = - pid_yaw.get_control()

        PID_YAW = constrain(PID_YAW, 0.3)

    else:

        PID_YAW = 0

    print(f"PID Control: PITCH={PID_PITCH}, ROLL={PID_ROLL}, YAW = {PID_YAW}")

    client.setVelXYYaw(PID_PITCH, PID_ROLL, 0)

# Выравнивание по высоте

def HeightRegulation(height_error):

    current_height = client.getHeightRange()

    target_height = current_height - height_error

    print(f'current_height is {current_height}')

    print(f'target_height is {target_height}')

    client.setHeight(target_height)

# Функция ограничения величины

def constrain(value, threshold):

    if value > threshold:

        value = threshold

    if value < -threshold:

        value = -threshold

    return value

# Функция для учета возможной ошибки инверсии знака оси z аруко маркера

def YawSign(angle, prev_yaw):

    prev_yaw = angle

    if abs(angle - prev_yaw) > 0.3:

        return prev_yaw

    else:

        return angle

def main():

    #Подключаемся

    print("connected?", client.connect(ip, port), "\n")

    #Сбрасываем скорости

    print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

    #Взлет

    print("takeoff?", client.takeoff(), "\n")

    time.sleep(5)

    #Ищем аруко маркер

    SearchForArucos()

    # Выравнивание относительно маркера

    ArucoRegulation()

    # Отлёт от старого маркера

    AvoidAruco()

    # Ищем следующий маркер

    SearchForArucos()

    # Выравнивание относительно маркера

    ArucoRegulation()

    # Устанавливаем цвет светодиода на красный

    client.setDiod(255.0, 0.0, 0.0)

    #Сбрасываем скорости

    print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

    #Посадка

    print("boarding?", client.boarding(), "\n")

    print("Main function completed successfully!")

client = Drone()

client.AddOnMessangeUtils(OnMes0)

if __name__ == "__main__":

    main()

Обнаружение и облет цветового блоба (BlobAvoid_PID_Reg.py)

Данный пример позволяет реализовать облет цветного объекта справа. Основные этапы работы:

1. Подключение к дрону.

Соединение с дроном по указанному IP и порту, произведение взлета.

2. Обнаружение блобов и выравнивание.

Дрон получает список обнаруженных объектов.Если размер объекта (блоба) превышает пороговое значение, он анализируется.

Вычисляется ошибка по оси ROLL относительно центра изображения. PID-регулятор корректирует движение дрона для центрирования.

3. Обход препятствия.

Если объект слишком большой (например, это стена), запускается алгоритм облета. Дрон летит вправо на 0.5 м.Затем летит вперед на 1 м. После этого смещается влево на 0.5 м, возвращаясь в исходную траекторию.

Если площадь объекта превышает определенный порог, дрон начинает движение к нему. Как только объект будет близко к дрону, запустится алгоритм облета.

Если площадь объектов меньше пороговой (минимальной), скорость обнуляется. В этом случае рекомендуется изменить пороговое значение площади объекта, либо перенести дрон ближе к объекту.

4. Посадка.

После завершения задачи дрон останавливается и выполняет посадку.

import time

from drone_control_api import Drone

from drone_control_api.pid import PID

import datetime

import math

ip = "10.42.0.1"

port = "1233"

last_time = None

def BlobRegulation():

   # Настройка PID регулятора для компоненты ROLL(в данном случае P-регулятор)

   pid_roll = PID(0.01, 0.0, 0.0)

   # Центральный пиксель изображения, приходящего с камеры

   camera_center_x = 320/2

   camera_center_y = 240/2

   # Целевая площадь блоба, при которой дрон приступит к облёту препятствия

   target_area = 2000

   # Минимальная площадь блоба

   threshold_area = 800

   # Инициализация площади блоба

   blob_area = 0

   while True:

       # Для каждого блоба из обнаруженных

       for blob in client.getBlobs():

           # Считаем приблизительную площадь блоба

           blob_area = blob["size"]["x"] * blob["size"]["y"]

           # Если площадь больше пороговой

           if blob_area > threshold_area:

               print(f"Обнаружен нужный блоб с центром: {blob['center']}, площадью: {blob_area}")

               # Считаем ошибку по ROLL компоненте

               roll_error = blob["center"]["x"] - camera_center_x

               # Выравниваемся и подлетаем с малой скоростью

               Regulation(roll_error,  pid_roll=pid_roll)

           else:

               # Сбрасываем скорость

               client.setVelXY(0, 0)

               print("Нет подходящих блобов. Обнуляю скорость")

       # Если площадь больше целевой

       if blob_area > target_area:

               print("Облёт препятствия")

               # Вызов функции облёта

               avoid()

               break

def avoid():

   # Обнуляем одометрию

   client.setZeroOdomOpticflow()

   # Летим вправо на 0.5 метра

   client.gotoXYdrone(0.0, -0.5)

   # Обнуляем одометрию

   client.setZeroOdomOpticflow()

   # Летим на 1 метр вперед

   client.gotoXYdrone(1.0, 0.0)

   # Обнуляем одометрию

   client.setZeroOdomOpticflow()

   # Летим влево на 0.5 метра

   client.gotoXYdrone(0.0, 0.5)

def Regulation(roll_error, pid_roll:PID):

   # Инициализация постоянной скорости PITCH

   PITCH_vel = 0.0

   # Если ошибка ROLL небольшая - летим вперёд с малой скоростью

   if roll_error < 15:

       PITCH_vel = 0.6

   # Регулировка скорости ROLL

   pid_roll.update_control(roll_error)

   PID_ROLL = pid_roll.get_control()

   # Ограничиваем скорость пороговым значением

   PID_ROLL = constrain(PID_ROLL, 1.0)

   print(f"Корректировка PID_ROLL : {PID_ROLL}")

   # Устанавливаем скорость

   client.setVelXY(PITCH_vel, PID_ROLL)

#Функция для ограничения значения переменной

def constrain(value, threshold):

   if value > threshold:

       value = threshold

   if value < -threshold:

       value = -threshold

   return value

client = Drone()

print("connected?", client.connect(ip, port), "\n")

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("takeoff?", client.takeoff(), "\n")

print("Blobs: ", client.getBlobs(), "\n")

print(BlobRegulation())

print("VelCorrect", client.setVelXYYaw(0,0,0),"\n")

print("boarding?", client.boarding(), "\n")