Осенью прошлого года компания 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();
///
/// Получить историю команд для текущей сессии
///
/// 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) Проброс навигационных данных в бизнес-сущности.
Дислокация
исходные коды доступны в репозитории
Ну и на последок небольшая демонстрация.