Осенью прошлого года компания 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
	/// <summary>
	/// Событие получения конфигурации от дрона
	/// </summary>
	event Action<ConfigData> ConfigurationReceived;

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

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

	#region methods
	/// <summary>
	/// Начало новой сессии
	/// </summary>
	/// <param name="droneAddress">адрес дрона</param>
	void StartSession(IPAddress droneAddress);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

public interface IParrot
{
	event Action<VideoFrame> 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) Проброс навигационных данных в бизнес-сущности.

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

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