Автономный полёт дрона с помощью Web-камеры

Зависание в точке

Если система определения координат настроена правильно - возможно тестирование автономного полёта. Программирование автономного полёта осуществляется на языке Python, с использованием библиотек ZuzaNanoRC и ZuzaNanoMocup.

После каждого запуска программы необходимо переподключить пульт к компьютеру!

Простейшая полётная миссия - осуществить взлёт, провисеть 10 секунд на месте, осуществить посадку. Данная миссия выполняется с помощью следующего кода (Fly0.py):

import ZuzaNanoRC
import ZuzaNanoMocap
import time
import cv2
 

deviceId=1
focus=1150
frameWidth=640
frameHeight=480
fps=40

try:
    Controller = ZuzaNanoRC.ZuzaNanoRC('COM4')
    if not Controller.Connect():
        print('Ошибка связи с портом '+Controller.PortName+'!')
        exit()

    # запуск Mocap класса автономного управления P I D 120 0.35 700
    ZuzaNanoMocap = ZuzaNanoMocap.ZuzaNanoMocap(deviceId, frameWidth, frameHeight, fps, focus,(0,142,0),(255,255,255), 3, cv2.COLOR_BGR2LAB, 0.100, True, 5,
        [0,0,-2.7], 127,[120, 0.35, 700],[210, 0.4, 0], 1.3).start() 
    print(">запуск Mocap класса автономного управления")
    time.sleep(1.0)
    if ZuzaNanoMocap.max_area==0:
        print('НЕ вижу ЖУЖУ!')
        exit()

    # бинд
    print(">Бинд")
    Controller.Bind()
    time.sleep (6)  
    
    # включили моторы
    Controller.SetThrottle(0)
    time.sleep(0.1)
    Controller.SetThrottle(127)
    time.sleep(1)
    #print(">включили моторы")

    # взлетаем
    Controller.SetThrottle(90)
    time.sleep(0.05)
    print('>Взлетаем, подключаем controller')
    ZuzaNanoMocap._dist_i=[0,0,0]
    ZuzaNanoMocap.write_avi = False #Установить значение True, если нужно записать видеопоток с камеры 
    ZuzaNanoMocap.Controller = Controller

    #пауза - зависание дрона
    print(">Взлетели, пауза...")
    time.sleep(30)

    # посадка, установим целевой Z = -10м  
    print("> Посадка! ")
    ZuzaNanoMocap.dd_Target[2]=-10
    time.sleep(3)

    ZuzaNanoMocap.stop()

except KeyboardInterrupt:
    print('>KeyboardInterrupt!')
    ZuzaNanoMocap.stop()
    Controller.SetThrottle(255)

Разберём этапы выполнения программы.

1. Импорт нужных модулей и инициализация контроллера пульта управления. Впишите свои значения focus (строка 8) и номер порта пульта (строка 14)

import ZuzaNanoRC
import ZuzaNanoMocap
import time
import cv2
 

deviceId=1
focus=1150
frameWidth=640
frameHeight=480
fps=40

try:
    Controller = ZuzaNanoRC.ZuzaNanoRC('COM4')
    if not Controller.Connect():
        print('Ошибка связи с портом '+Controller.PortName+'!')
        exit()

2. Инициализация класса ZuzaNanoMocap для автономного управления. В массив начальных координат [0, 0, -2.7] вместо -2.7 запишите своё значение расстояния от камеры до пола

   # запуск Mocap класса автономного управления P I D 120 0.35 700
   ZuzaNanoMocap = ZuzaNanoMocap.ZuzaNanoMocap(deviceId, frameWidth, frameHeight,
       fps, focus,(0,142,0),(255,255,255), 3, cv2.COLOR_BGR2LAB, 0.100, True, 5,
       [0,0,-2.7], 127,[120, 0.35, 700],[210, 0.4, 0], 1.3).start() 
    print(">запуск Mocap класса автономного управления")
    time.sleep(1.0)
    if ZuzaNanoMocap.max_area==0:
        print('НЕ вижу ЖУЖУ!')
        exit()

3. Привязка пульта к дрону

Controller.Bind()

4. Взлёт и передача управления классу ZuzaNanoMocap (с помощью присваивания ZuzaNanoMocap.Controller = Controller)

    # включили моторы
    Controller.SetThrottle(0)
    time.sleep(0.1)
    Controller.SetThrottle(127)
    time.sleep(1)
    #print(">включили моторы")

    # взлетаем
    Controller.SetThrottle(90)
    time.sleep(0.05)
    print('>Взлетаем, подключаем controller')
    ZuzaNanoMocap._dist_i=[0,0,0]
    ZuzaNanoMocap.write_avi = False #Установить значение True, если нужно записать видеопоток с камеры 
    ZuzaNanoMocap.Controller = Controller

5. Ожидание 30 секунд. Класс ZuzaNanoMocap удерживает дрон в целевой точке, в фоновом режиме

    #пауза - зависание дрона
    print(">Взлетели, пауза...")
    time.sleep(30)

6. Посадка и остановка обработки видеопотока с камеры

    # посадка, установим целевой Z = -10м  
    print("> Посадка! ")
    ZuzaNanoMocap.dd_Target[2]=-10
    time.sleep(3)

    ZuzaNanoMocap.stop()

Перед запуском программы необходимо установить дрон вертикально под центром камеры, включить пульт, затем включить дрон.

После взлёта программа осуществляет зависание дрона в точке.

Полёт по квадрату

После того как дрон стабильно зависает в точке - можно писать программы произвольных полётных миссий. В качестве примера ниже приведён фрагмент кода - полёт по квадрату. Начальные координаты дрона (X,Y,Z) указаны в метрах относительно центра камеры: ZuzaNanoMocap.dd_Target = [0,0,-2.7]. После взлёта следующий код осуществляет полёт дрона по квадрату(Fly square.py):

    for i in range(3):
        for y in range(10,-11,-1):
            ZuzaNanoMocap.dd_Target[1]=y/50 #50
            time.sleep(0.3)
        time.sleep(0.5)
        for x in range(10,-11,-1):
            ZuzaNanoMocap.dd_Target[0]=x/50
            time.sleep(0.3)
        time.sleep(0.5)
        for y in range(-10,11,1):
            ZuzaNanoMocap.dd_Target[1]=y/50
            time.sleep(0.3)
        time.sleep(0.5)
        for x in range(-10,11,1):
            ZuzaNanoMocap.dd_Target[0]=x/50
            time.sleep(0.3)
        time.sleep(0.5)

После завершения полёта по каждой из сторон квадрата программа ставит паузу 0.5 секунды (time.sleep(0.5)) для того, чтобы дрон успел стабилизироваться в точке вершины квадрата.

Варьируя паузы и графики изменения координат - возможно выполнять произвольные полётные миссии.

Желаем Вам успешных полётов!