Разработка для коптеров

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



Коптеры разные. Единого стандарта нет. В основе любого коптера лежит полетный контроллер который аккумулирует данные с датчиков, управляет стабилизацией и положением в пространстве. Управляя с пульта вы отправляете команды полетному контроллеру, в результате чего изменяются обороты двигателей, что позволяет перемещаться в воздухе. В более продвинутых коптерах кроме пульта, управление доступно с наземной станции (Ground Station). Использование бортового мини-компьютера с подключением непосредственно к полетному контролеру сочетает в себе достоинства ручного пульта и наземной станции, позволяет автоматизировать полет и взаимодействовать с внешним миром.

Для моего проекта требовалась готовая платформа с возможностью расширения. В то же время хотелось что бы все работало «из коробки». Необходимо поднимать 500-600 грамм груза, в том числе бортовой мини-компьютера для обработки данных непосредственно в полете. Ну и конечно цена должна быть в разумных пределах. В то время вышел Solo — новая модель от 3DR на Pixhawk 2, в которой помимо «полного фарша», установлен мини-компьютер под управлением Linux, связь по WiFi, возможность подключения непосредственно к полетному контроллеру и к бортовому компьютеру. Грузоподъемность до 700 грамм. В общем то что нужно и через некоторое время он был у меня.




При включении контроллер создает WiFi сеть, коптер доступен по адресу 10.1.1.10, а контроллер 10.1.1.1. и в том и другом случае есть ssh доступ:

ssh root@10.1.1.10
пароль TjSDBkAu

Внутри используется Linux, который базируется на Yocto Project 1.5.1 на ARM Cortex A9/Freescale i.MX6. Система уменьшается на 8Gb microSD карте из которых 7.2Gb занимает раздел под полетные логи. По умолчанию, для операционки и кода самый минимум. Если собрались что то изменять в системе, то обязательно сделайте бэкап. Для этого придется снять двигатели, открыть корпус и достать основную плату. Размер разделов на microSD в этом случае придется изменить или заменить карту на побольше предварительно скопировав данные через ddresque и изменив размеры ext3 разделов.

В нижней части Solo есть разъем через который выведены выходы USB, UAVCAN, UART для доступа к imx6 и Pixhawk. С платы доступны дополнительные выводы в том числе из под Pixhawk, но придется взять в руки паяльник. Так например USB кабель служит для подключения сторонних устройств и работает в режиме OTG хост, для подключения внешних устройств при замыкании перемычки или как устройство при подключении к внешнему компьютеру. Для тех кто не хочется возится с паяльником разработали Accessory Breakout Board на которой большинство выводов уже есть, остается только использовать соответствующие разъемы.



Когда нет помех, сигнал от контроллера до коптера достигает больше 500 метров, 800 по спецификации, что позволяет управлять в режиме онлайн с ноутбука через WiFi сеть. При хорошей погоде коптер наблюдается на расстоянии до 300-400 метров, все что дальше превращается в точку и ручное управление доступно только с помощью FPV или автоматики. На дальние и сложные дистанции лучше заправлять скрипты управления непосредственно на Solo что бы исключить проблемы со связью и видимостью. По умолчанию доступна загрузка скриптов по sftp, rsync, а так же через утилиту solo. Для скриптов крайне желательно использовать virtualenv, иначе версии пакетов могут конфликтовать с системными.

Mavlink (Micro Air Vehicle Communication Protocol) это открытый коммуникационный протокол, который поддерживают большинство открытых полетных контроллеров: APM/Pixhawk, MultiWii, Navio и другие. С помощью mavlink можно отправлять управляющие команды, задавать миссии и получать телеметрию. Со времени своего появления его начали использоваться не только для микро авиации (copter, plane), но и для управления микро- машинами, катерами и даже подводными лодками. В зависимости от типа транспортного средства и используемого контроллера, функционал немного отличается.

Для python протокол mavlink реализован в модуле pymavlink. Напрямую его использовать не очень комфортно и в DroneKit частично реализовали свою обертку. Частично, потому что на практике, сделано далеко не все, и к mavlink постоянно приходится обращаться. Нереализованные возможности mavlink’a есть и в полетных контроллерах, т.е. наличие того или иного функционала приходится проверять и тестировать. Некоторые команды и режимы могут быть не только не реализованы, но и иметь отличные от других названия. Поэтому лучше разрабатывать под определенный контроллер и быть уверенным в его будущей поддержке.

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

#!/usr/bin/python
#-*- coding: utf-8 -*-

from dronekit import connect

# Подключаемся к коптеру
# объект нашего коптера для работы через DroneKit.
vehicle = connect('udpin:0.0.0.0:14550', wait_ready=True)

# Атрибуты коптера
print "Vehicle state:"
# Объект с координатами широты, долготы, высоты и относительной высоты. 
# Высота считается относительно уровня моря, а относительная высота применяется к стартовой позиции.
print " Global Location: %s" % vehicle.location.global_frame
print " Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
print " Local Location: %s" % vehicle.location.local_frame
# Позиция в кординатах pitch, yaw, roll
print " Attitude: %s" % vehicle.attitude
# Текущий вольтаж, ток и оставшийся уровень заряда
print " Battery: %s" % vehicle.battery
# Время последней удавшаяся проверки связи с коптером. 
print " Last Heartbeat: %s" % vehicle.last_heartbeat
# Направление, в градусах относительно севера
print " Heading: %s" % vehicle.heading
# Скорость по земле
print " Groundspeed: %s" % vehicle.groundspeed
# Скорость в воздухе
print " Airspeed: %s" % vehicle.airspeed
# можем запускать двигатели или нет
print " Is Armable?: %s" % vehicle.is_armable
# Запущены ли двигатели
print " Armed: %s" % vehicle.armed
# Режим в котором сейчас находимся
print " Mode: %s" % vehicle.mode.name

vehicle.close()
print "Done."

Режимы это набор заранее определенных действий и доступного функционала. Так в ALT_HOLD будет сохраняться заданная высота и доступны функции перемещения, AUTO запускает миссии, а LAND сажает коптер и выключает моторы. В некоторых режимах необходим GPS и GPS Lock перед стартом, другие работают без него. При необходимости можно сделать собственные режимы и запрограммировать необходимое поведение.

Пора взлетать:

#!/usr/bin/python
#-*- coding: utf-8 -*-

import time
from dronekit import connect, VehicleMode, LocationGlobalRelative

# Подключаемся
vehicle = connect('unpin:10.1.1.10:14550', wait_ready=True)

def arm_and_takeoff(aTargetAltitude):
    """ Запускае двигатели и взлетаем до высоты aTargetAltitude
    """
    print "Предполетные проверки"
    while not vehicle.is_armable:
        print "Ждем коптер..."
        time.sleep(1)

    print "Запускаем двигатели"
    # Устанавливаем GUIDED режим
    vehicle.mode    = VehicleMode("GUIDED")
    # Запускаем моторы
    vehicle.armed   = True

    #  Ждем пока моторы раскрутятся. 
    # Поскольку все происходит в реальном мире, нужно не забывать что на выполнение действий требуется время.
    while not vehicle.armed:
        print " Ждем моторы..."
        time.sleep(1)

    print "Взлет!"
    # Взлетаем до нужной высоты
    vehicle.simple_takeoff(aTargetAltitude) 

    # Даем задание подняться до нужной высоты
    #  и… ждем пока поднимемся.
    while True:
        print " Текущая высота: ", vehicle.location.global_relative_frame.alt
        # Что бы не скучно было, смотрим как высоко уже поднялись
        if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:
            print "Поднялись на %d метров" % vehicle.location.global_relative_frame.alt
            break
        time.sleep(1)
    
    print "Достигли заданной высоты"

# Запускаемся и поднимаемся на 20 метров
arm_and_takeoff(20)

После коннекта, ждем пока все предполетные проверки будут закончены и vehicle.is_armable станет True. Далее переключаемся в режим GUIDED который позволяет последовательно задавать точки при достижении которых коптер будет ждать «дальнейших указаний». Коэффициент 0.95 не обязателен, но точность датчиков не идеальна, и это приходится учитывать. С другой стороны это позволяет не терять время, поскольку присутствует инерция и скорость в полете не равномерна. Последние сантиметры займут несколько лишних секунд, а у нас батарейка быстро садится и ± лишний метр высоты не критичен.

Настало время куда-нибудь улететь слетать. Задаем широту, долготу и высоту места куда отправим коптер.

# Задаем координаты нужной точки
a_location = LocationGlobalRelative(-27.114346, -109.357912, 20)
# полетели 
vehicle.simple_goto(a_location)

Не забудьте поменять координаты на ваши! Если вдруг забыли, то перехватить управление Solo можно кнопкой Pause на пульте или подняв оба стика в правый верхний угол. В крайнем случае, я надеюсь что вы подключили планшет и получаете данные с gps, а на борту можно рассмотреть номер телефона для связи.

В полете можно контролировать некоторые параметры, например скорость:

# Путевая скорость, м/с
vehicle.groundspeed = 7.5

В данном случае нет механизма нотификации о достижении заданной цели… помните что коннект mavlink’a мы делали по UDP? те наше сообщение может уйти в никуда, и такое иногда случается, особенно на длинных дистанциях.

Зная скорость и расстояние рассчитать время прибытия и поставить таймер не проблема. В идеальном случае это сработает. Для коптера любой ветер, встречный, попутный, боковой вносит свои коррективы. Штиль «на земле» совсем не означает что на 30-50-100 метрах ветра нет, поэтому линейный расчет будет очень и очень приблизительный.


Более точный способ это конечно проверять позицию по GPS/Glonass/Gallileo/Beidou. В зависимости от используемого чипа и его настроек, проверка имеет смысл не чаще чем 1-10 раз в секунду, иногда интервал больше. При этом точность расположения может быть плюс-минус несколько метров.

def is_arrived(lat, lon, alt, precision=0.3):
    
    # текущая позиция
    veh_loc = vehicle.location.global_relative_frame
   
    # получаем данные в метрах
    diff_lat_m = (lat - veh_loc.lat) * 1.113195e5
    diff_lon_m = (lon - veh_loc.lon) * 1.113195e5
    diff_alt_m = alt - veh_loc.alt

    # отклонение
    dist_xyz = math.sqrt(diff_lat_m**2 + diff_lon_m**2 + diff_alt_m**2)

    if dist_xyz < precision:
        print "Прибыли на место"
        return True
    else:
        print "Еще не долетели"
        return False

# проверяем долетели или еще нет
while not is_arrived(lat, lon, alt):
    # ждем еще 3 секунды
    time.sleep(3)


# прилетели на место и сделали что-нибудь полезное
time.sleep(10)

Достигнув заданной точки, возвращаемся домой. По аналогии, задать координату и так же по прибытии посадить используя режим LAND — это сложный путь. Дело в том что при старте данные о начальной позиции сохраняются на все время полета, а для возврата домой существует режим RTL (Return To Launch). В RTL коптер сначала поднимается до «высоты возврата» RTL_ALT, это позволяет исключить возможность столкновения с деревьями и высотными объектами, возвращает коптер в точку старта, автоматически сажает девайс и выключает моторы.

print "Возвращаемся"
vehicle.mode = VehicleMode("RTL")

Через некоторое время коптер прилетит и после посадки радостно пропиликает о прибытии. Можно выдохнуть… все хорошо! Результат на видео:




Проблема в том что может и не прилететь, не долететь, или прилететь не туда. Любая ошибка стоит дорого. И это не только коптер, но и груз на нем. Кроме чисто программных ошибок и проблем с оборудованием, есть проблемы с обслуживанием. Например желательно наличие ровной и чистой площадки для взлета/посадки, можно сесть одной ногой на камень… и перевернуться. Даже со взлетом появляются определенные проблемы, например если нужно взлететь с яхты которая качается на волнах.

Что бы не было так больно от падений и прочих неприятностей скрипты тестирум в SITL (Software in the Loop). SITL это симулятор полетов, позволяет отработать различные ситуации, в том числе некоторые погодные условия, особенности местности, сбои в работе GPS, потерю сигнала и т.д. Работает SITL не всегда корректно, но в целом неплохо. Полноценное тестирование заменить в любом случае не получится. Для визуализации в 3D используется связка SITL с FlightGear или jMAVSim.

Взлететь, отметится на точке и вернутся — это просто. Много полезных функции уже реализовано для миссий, полеты по радиусу, следование за объектом, управления типовым оборудованием: камерой и подвесом. Однако коптер летит «в слепую», не видит и не анализирует обстановку. Приходится задавать максимальные параметры как в RTL или использовать различные датчики, камеры, дополнительное оборудование для расчетов, в конце концов делать свои датчики и системы оценки ситуации. Поэтому желательно иметь хотя бы общее представление об электронике, робототехнике, согласовании уровней, протоколах передачи данных, датчиках. Все это так или иначе потребуется если будете углубляться в эту тему.