AR.Swift — разбираемся в АПИ Ar.Drone 2.0


Осенью прошлого года компания Microsoft объявила о проведении конкурса по разработке приложения для управления квадрокоптером AR.Drone под одну из своих новых платформ (WinRT и WP 8). Сам конкурс своим ходом заглох: пройдя первый отборочный тур, мы так и не дождались самого коптера, который нам должны были прислать для реализации предложенной идеи.
Однако желание воплотить её в жизнь было достаточно большим, поэтому, раздобыв самостоятельно этот самый AR.Drone, мы (да, нас несколько человек) приступили к реализации.


Вначале предстояло решить вопрос о взаимодействии с дроном. Так как целевой проект был на C#, то нам нужна была библиотека под .NET Framework. Таковых оказалось не очень много, и все они не предоставляли необходимую нам функциональность в полной мере (в частности, неполные навигационные данные, отсутствие доступа к уровню команд и т.п.). Кроме того, было интересно и самим разобраться с программным обеспечением дрона. Поэтому мы решили изобрести паровой велосипед с двойным турбонаддувом и активной подвеской и решили реализовать свою обертку для работы с железкой.

Для начала несколько слов об устройстве дрона.
Управление дроном происходит с помощью AT-команд. Команды должны приходить на дрон каждые 2 секунды, иначе устройство решит, что связь с хостом утеряна и он перейдет в режим экстренной посадки . Команда выполняется на дроне до тех пор, пока не придет следующая команда. Таким образом, для того что бы выполнить некоторую команду, вы должны посылать её каждые 2 секунды (или чаще), до тех пор пока не посчитаете, что команда выполнилась, а затем необходимо отправить специальную команду поддержки соединения , чтобы дрон не перешёл в экстренный режим. Для команд маневрирования рекомендуется отправлять их на дрон с интервалом в 30 миллисекунд для обеспечения плавного движения квадрокоптера.
Взаимодействие с дроном осуществляется с помощью 5 каналов связи:
• На порт 5556 дрона по протоколу UDP отправляются команды управления.
• На порт 5554 дрона по протоколу UDP отправляется пакет для инициализации канала связи пересылки навигационных данных.
• На порт 5554 хоста по протоколу UDP присылаются навигационные данные.
• С порт 5555 дрона по протоколу TCP присылается видеопоток с камеры дрона.
• С порт 5559 дрона по протоколу TCP присылается конфигураци.

Реализация.
Ознакомившись с документацией, мы приступили к реализации команд. Были реализованы следующие команды:
Команда: AT*CALIB
Назначение: калибровка магнитометра
Параметры: тип калибруемого устройства

Команда: AT*COMWDG
Назначение: команда поддержки соединения (NOOP)

Команда: AT*CONFIG
Назначение: установка конфигурационного параметра в указанное значение
Параметры: Parameter – конфигурируемый параметр, Value – устанавливаемое значение

Команда: AT*CONFIG_IDS
Назначение: режим мультиконфигурации, позволяет дрону переключаться между разными наборами конфигураций
Параметры: Current Session ID, Current User ID, Current Application ID – имена (не обязательно идентификаторы) текущих сессии подключения к дрону, пользователя, приложения. Эти параметры строковые.

Команда: AT*CTRL
Назначение: запрос текущей конфигурации и навигационных данных. В отличие от всех остальных AT-команд, у этой команды нет последовательного номера в серии, только параметр, который определяет, какие данные нужно получить.
Параметры: один параметр, который принимает значения:
4 – запрос конфигурации,
5 – запрос навигационных данных,
6 – запрос списка идентификаторов пользовательских конфигураций.

Команда: AT*FTRIM
Назначение: калибровка горизонта

Команда: AT*PCMD
Назначение: управление движением дрона в системе координат, связанной с его продольной осью. Параметры Roll, Pitch, Vertical Speed и Angular Speed имеют две важные особенности:
— они задаются не в абсолютных значениях, а в долях от максимальных значений; например, если Vertical Speed передать 0,5, это будет означать, что нужно поднимать вверх со скоростью, равно 0,5 от максимальной, установленной в текущей конфигурации;
— дрону передаются не сами вещественные числа, а целые 32-битовые числа, имеющие такое же битовое представление, что и соответствующие вещественные числа в формате IEEE-754.
Параметры: Flags – 32-битовое целое число, младшие три бита которого задают режим управления дроном: полёт, зависание, режим CombinedYaw (включён / выключен), режим AbsoluteControl (включён / выключен).
Roll – крен дрона (наклон в направлении влево – вправо.
Pitch – тангаж дрона (наклон в направлении вперёд – назад). Эти два параметра определяют движение дрона (его скорость) в горизонтальной плоскости.
Vertical Speed – вертикальная скорость.
Angular Speed – угловая скорость.

Команда: AT*PCMD_MAG
Назначение: управление движением дрона в системе координат, связанной с магнитным полем Земли
Параметры: все те же самые, что и у AT*PCMD, но добавляются ещё два.
Magneto Psi – курс дрона относительно магнитного севера, задаётся в долях от полуоборота (0 – север, +0,5 – восток, +1 – юг, –0,5 – запад, –1 – юг).
Magneto Psi Accuracy – точность позиционирования по магнитометру в градусах.

Команда: AT*REF
Назначение: управление взлетом / посадкой
Параметры: 32-битовое целое число, все разряды которого, кроме 8-го и 9-го, зарезервированы и должны быть выставлены так, как указано в инструкции, иначе дрон не взлетит. Комбинации 0 и 1 в оставшихся 8-м и 9-м разрядах задают следующие действия: взлёт, посадка, переключение в экстренный режим или выход из него, поддержание экстренного режима.

Некоторые действия (например, получение конфигурации от дрона или взлёт) требуют выполнения серии команд с ожиданием изменения состояния дрона. Чтобы упростить рутинные операции, мы ввели сущность «скрипт». Скрипт – это последовательность команд и логика по контролю состояния дрона, которые описывают некоторую стандартную процедуру.
На текущий момент мы реализовали два скрипта: получение конфигурации и взлёт дрона.
Непосредственное взаимодействие с устройством производится через объект диспетчера.

public interface IDispatcher
{
	#region events
	/// 
	/// Событие получения конфигурации от дрона
	/// 
	event Action ConfigurationReceived;

	/// 
	/// Событие получения навигационных данных
	/// 
	event Action NavigationDataReceived;

	/// 
	/// Событие получения видео фрейма от дрона
	/// 
	event Action VideoDataReceived;
	#endregion

	#region methods
	/// 
	/// Начало новой сессии
	/// 
	/// адрес дрона
	void StartSession(IPAddress droneAddress);

	/// 
	/// Завершить текущую сессию
	/// 
	void EndSession();

		/// 
		/// Поместить команду в очередь команд
		/// 
		/// команда для выполнения
		/// идентификатор комманды (ВНИМАНИЕ! Это внутренний идентификатор для ссылки в диспетчере, он никак не связан с реальным идентификатором команды на дроне!)
		uint PushCommand(ATCommand command);

		/// 
		/// Поместить команду в очередь команд
		/// 
		/// команда для выполнения
		/// время, в течении которого должна выполняться команда
		/// идентификатор комманды (ВНИМАНИЕ! Это внутренний идентификатор для ссылки в диспетчере, он никак не связан с реальным идентификатором команды на дроне!)
		uint PushCommand(ATCommand command, TimeSpan executeDuration);

		/// 
		/// Очистить очередь команд
		/// 
		void ClearCommandQueue();

		/// 
		/// Остановить выполнение текущей команды
		/// 
		void StopCommand();

		/// 
		/// Удалить команду из очереди
		/// 
		/// идентификатор удаляемой команды
		void RemoveCommand(uint id);

		/// 
		/// Запуск канала для получения конфигурации
		/// 
		void BeginReceiveConfiguration();

		/// 
		/// Завершение работы канала для получения конфигурации
		/// 
		void CancelReceiveConfiguration();

		/// 
		/// Начать прием видео от дрона
		/// 
		void StartVideoCapture();

		/// 
		/// Завершить прием видео от дрона
		/// 
		void StopVideoCapture();

	/// 
	/// Получить состояние диспетчера
	/// 
	/// В случае если в очереди команд нет команд для выполнения возращается true (диспетчер простаиват), иначе false
	bool IsIdling();

	/// 
	/// Получить историю команд для текущей сессии
	/// 
	/// 
	IEnumerable GetSessionHistory();
	#endregion
}

Стоит отметить об одной особенности реализации диспетчера. Методы PushCommand не выполняют команду сразу. При вызове этого метода команда помещается в очередь команд и будет выполнена в промежутке времени 0..30 мс, если очередь была пуста, либо в промежутке N..N + 30 мс (где N – время выполнения всех команд, находящихся в очереди впереди), если в очереди присутствуют команды.

Из-за особенности реализации программного обеспечения дрона каждый вызов StartSession сбрасывает счётчик команд. Кроме того, выполняется инициализация канала получения навигационных данных. После завершения инициализации в диспетчере работают два фоновых потока – для получения навигационных данных и для обработки очереди команд.
Вызов метода StartVideoCapture запускает еще один фоновый поток, который обрабатывает получение видеоданных с дрона, а метод StopVideoCapture завершает его.
Метод BeginReceiveConfiguration инициирует работу дополнительного потока, который обрабатывает канал связи для получения конфигурации. Получение конфигурации будет обрабатываться до тех пор, пока пользователь не вызовет метод CancelReceiveConfiguration.

В рамках сессии пользователь имеет доступ к истории всех отправленных команд. Для возможности сохранения/восстановления действий пользователя для всех команд сделаны обёртки, которые позволяют их сериализовать/десериализовать в xml-файл.

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

public interface IParrot
{
	event Action VideoFrameDecoded;

	// устанавливает соединение с дроном и инициализирует сессию
	void Connect(IPAddress address);

	// завершение работы с дроном (завершение сессии)
	void Disconnect();

	// Взлет
	void Takeoff();

	// Посадка
	void Landing();

	// запуск процесса получения видео потока с дрона. Видео данные доступны через событые VideoFrameDecoded
	void BeginCaptureVideo();

	// остановка процесса получение видео потока с дрона
	void StopCaptureVideo();

	// получить объект диспатчера, для получение более низкоуровневого доступа
	IDispatcher GetDispatcher();

	// получить конфигурацию дрона
        DroneConfig GetConfiguration();

        // будет дополняться
}

Резюме
Что реализовано:
1) Реализованы основные команды управления дрона
2) Реализовано получение конфигурации
3) Реализовано получение навигационных данных и их парсинг (на низком уровне, без проброса в бизнес-сущности)
Что не реализовано:
1) Получение и обработка видео-потока. Непосредственно получение реализовано, но сейчас мы обдумываем проблему с декодированием. У нас целевая платформа была WP 8, но под нее мы не нашли (может плохо искали) ffmpeg, который повсеместно используется для декодирования в аналогах на других платформах.
2) Проброс навигационных данных в бизнес-сущности.

Дислокация
исходные коды доступны в репозитории

Ну и на последок небольшая демонстрация.


Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна конкурс манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение