Программирование приложений на android 

Правило для вращения вертолета вокруг вертикальной оси

Правило выполняется циклически до тех пор, пока yaw = true. Состоит их 3 частей.

В первой части вычисляем угол относительно вертикальной оси когда поворот считается выполненным. Для расчета используем заданный угол поворота и текущее значение угла относительно оси.

Заданный угол поворота может иметь значение в интервале от нуля до ста восьмидесяти градусов. Начальный угол относительно оси определяется с помощью данных принятых с вертолета

Int angle_start = (int) get_float(buf_rcv, NAVDATA_YAW)/1000;

Где buf_rcv – буфер куда помещен принятый с вертолета пакет, NAVDATA_YAW – смещение первого байта величины угла относительно начала пакета, get_float – процедура выборки четырех байтов в которых хранится значение величины угла

Далее вычисляем angle_final.

Чтобы придумать алгоритм вычисления надо посмотреть как он определен. Нулю градусов соответствует направление на север, по ходу часовой стрелки идет рост угла от нуля до ста восьмидесяти, против часовой стрелки идет уменьшение значений угла от нуля до минус ста восьмидесяти. Таким образом значение угла меняется скачком от плюс 180 к минус 180 в направлении на юг.

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

Процедуры выхода из вращения

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

Но тут есть проблема связанная с дискретностью процесса измерения. При большой скорости вращения и большом финальном угле, близком к 180 градусам есть вероятность проскочить область положительных углов больших чем финальный и тогда вращение никогда не остановится.

В этом случае выход из вращения будет выполнен в случае если финальный угол больше нуля , текущее значение меньше начального и текущий угол отрицателен.
Последний случай это когда финальный угол меньше нуля. Выход выполняется при условии, что текущий угол больше финального и текущий угол отрицательный.

Код:

// Правило первое. Поворот вокруг вертикальной оси на заданный угол // yaw - логическая величина, которая позволяет в цикле обрабатывать правило // когда правило обработанно yaw присваиваем значение false              if (yaw) {                                  if (n_yaw==0){                    angle_start = (int) get_float(buf_rcv, NAVDATA_YAW)/1000;                     send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                     send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                     Thread.sleep(INTERVAL);                                                               angle_final = angle_start + psi_angle;                                       if (angle_final > 0) {                        int ost = angle_final % 180 ;                        int chast = angle_final / 180 ;                         if (chast > 0) {                            angle_final = ost - 180 ;                         }                     }                     if (angle_final==180) angle_final = -180;                      n_yaw++;                 }                 send("AT*PCMD=" +(seq++)+",0,0,0,0,1056964608");                 int angle_current =  (int) get_float(buf_rcv, NAVDATA_YAW)/1000;                                  if (angle_current > angle_final & angle_final > 0 ){                    yaw=false;                                        send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                    Thread.sleep(INTERVAL);                     }                                  if (angle_current < angle_final & angle_final > 0 & angle_current < 0 ){                                        yaw=false;                                        send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                    Thread.sleep(INTERVAL);                                     }                                  if (angle_current > angle_final & angle_final < 0 & angle_current < 0 ){                    yaw=false;                                        send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                    Thread.sleep(INTERVAL);                 }                 Thread.sleep(INTERVAL);                               }

Ошибка в условии

Код:

if (angle_current < angle_final & angle_final > 0 & angle_current < 0 ){                                        yaw=false;                                        send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                    Thread.sleep(INTERVAL);                                     }

Надо так

Код:

if (angle_final > 0 & angle_current < 0 & angle_current < angle_start){                                        yaw=false;                                        send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");                    Thread.sleep(INTERVAL);                                     }

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

Переписка с Arman

Он пишет :


Вот мне нужны такие задачи!
1) Среда разработки приложений для AR.Drone Parrot
2) Как программируется?
3) Примеры программ
4) Найти литературы
5) Алгоритмы

Как мне сказали как таковой среды разработки нету но есть SDK! Что такое это воопще SDK?)
Программируется через С, C++, Java, Python верно?
В этом SDK файле заключены и примеры программ, и литература(токо она на англиском это печально), и алгоритмы тоже в этом файле верно?


Я отвечаю :

SDK это набор инструментов облегчающих программисту управление устройством. Это программа преобразующая инструкции высокого уровня в инструкции которые понимают устройства вертолета - моторы, светодиоды и так далее.

Например, чтобы вертолет взлетел надо послать ему команду

"AT*REF=" + (seq++) + ",290718208"

seq - порядковый номер команды

А SDK уже переведет эту команду на язык низкого уровня, на язык управления моторами вертолета

Похоже на драйвер к устройству ПК

Вот здесь можно скачать Руководство пользователя на английском

[внешняя ссылка]

-------------------------


Программировать поведение drone можно на любом языке которым Вы владеете.

Можно себе представить вертолет как компьютерную систему которая создает wifi сеть и открывает несколько портов для общения. Чтобы подключиться к этой сети Ваш ПК должен иметь wifi адаптер

На порт 5556 посылаются команды управляющие поведением вертолета. Команда это текстовая строка в кодировке ASCII.

Перед отправкой команды она преобразуется в байтовый массив и отсылается в виде дейтаграммы, то есть используется протокол UDP

Для получения с вертолета навигационных данных используется его (вертолета) порт 5554

--------------------------------

как круто-то всё!
Вам, наверное, раз плюнуть написать программу-звонилку для андроид-двух-симочников, чтоб в телефонной книжке можно было один раз указать с какой симки звонить?
ничего нормально работающего на просторах инета не нашел.

Не, не раз плюнуть. Это надо в тему входить. И потом, чтобы вышло хорошо, нужно чтобы тема "приягивала" . А у меня к "звонилкам" душа не лежит.

Мы пилоты

братья?

Да

Мы летим, ковыляя во мгле

Не хочет вертолет прямо летать. Куда то в бок его тянет все время.

То есть команды впред/назад и вправо/влево исполняются нечетко. Почти что в произвольном направлении летит вертолет

Говорят, что надо удалить файл trims.bin в папке data

Вопрос от Arman

А есть ли SDK на windows?

Ответ

Управление вертолетом происходит с помощью передачи ему AT команд. Примеры этих команд я уже приводил в этой теме. Они избавляют разработчика (developer) от необходимости разбираться с аппаратным устройством вертолета

Кроме того фирма Parrot разработала две библиотеки, ARDroneLIB и ARDroneTool, которые осуществляют так называемый API-интерфейс к управлению. То есть на входе этих библиотек API-команды, а на выходе AT-команды, которые собственно и управляют вертолетом.

Эти две библиотеки называют SDK. Они написаны на языке C. То есть будут работать в любой системе, в частности под Windows

Я не использую SDK для управления вертолетом. Пользуюсь AT-командами.

Что такое AT команды
Текстовая строка в кодировке ASCII. Перед всеми командами ставится префикс AT* или в кодах ASCII - 41,54,2a, затем идет команды, знак равно, порядковый номер команды (sequence number) и далее может следовать список аргументов команды, разделенных знаком запятая.
Неправильные команды игнорируются.
Программа вертолета обрабатывающая команды клиента следит, чтобы номер обрабатываемой команды был больше, чем номер последней обработанной. Если это не так, то команда не выполняется
Счетчик порядкового номера вертолета сбрасывается в единицу в случае если команды не приходят от клиента в течении двух секунд

Большинство команд имеют аргументы. Аргументы могут быть следующих типов
1. Целое число в десятичном представлении. Например, порядковый номер команды
2. Строка. В этом случае значение заключаются в двойные кавычки
3. Число с плавающей запятой в формате IEEE-754 с одинарной точностью. Для того чтобы представить число с плавающей запятой в формате IEEE-754 можно воспользоваться сетевым сервисом [внешняя ссылка] Например, нужно преобразовать число 0.1 в форму IEEE-754. Вводим в поле Decimal Floating-Point это значение и нажимаем кнопку Rounded. Далее смотрим полученное значение в поле Hexadecimal для чисел Single precision. Полученное значение в шестнадцатиричном виде (3DCCCCCD) можно преобразовать к десятичному виду с помощью калькулятора Windows. Получим число 1036831949. Для преобразования отрицательного числа сначала преобразуем его в шестнадцатиричное число с помощью сервиса, затем отнимаем полученное значение от числа FFFFFFFF в калькуляторе Windows, преобразуем в десятичное значение и к нему добавляем единицу.

Наверное самая необходимая команда для управления вертолетом это команда PCMD. У этой команды пять аргументов. Первый аргумент это код режима управления вертолетом. Аргументы со второго по пятый это числа от минус единицы до плюс единицы, которые представляют процент от максимальной скорости в заданном направлении.

Второй аргумент задает скорость движения вправо/влево, третий аргумент скорость в направлении вперед/назад, четвертый аргумент это скорость движения вверх/вниз, пятый – скорость движения вокруг вертикальной оси

Если хочется покомандовать, а не кем, то можно покомандовать drone

Вот консольная программа на java с помощью которой можно поморгать светодиодами

Код:

import java.awt.Frame; import java.awt.event.KeyEvent; import java.awt.event.KeyListener; import java.awt.event.WindowAdapter; import java.awt.event.WindowEvent; import java.net.DatagramPacket; import java.net.DatagramSocket; import java.net.InetAddress; import java.util.StringTokenizer; public class Test1 extends Frame implements KeyListener{     static final int AT_PORT      = 5556;         static InetAddress inet_addr;               static int seq = 1; //Send AT command with sequence number 1 will reset the counter     boolean space_bar = false; //true = Takeoff, false = Landing             static DatagramSocket socket_at;             public Test1(String name, String args[], InetAddress inet_addr) throws Exception {         super(name);         //System.out.println("Speed: " + speed);        addKeyListener(this);                         setSize(320, 160);        setVisible(true);        addWindowListener(new WindowAdapter() {            public void windowClosing(WindowEvent e) {           System.exit(0);            }          });             }         public static void main(String[] args) throws Exception{                // Создаем значение для переменной ip_addr               String ip = "192.168.1.1";        StringTokenizer st = new StringTokenizer(ip, ".");        byte[] ip_bytes = new byte[4];        if (st.countTokens() == 4){             for (int i = 0; i < 4; i++){           ip_bytes[i] = (byte)Integer.parseInt(st.nextToken());            }        }        inet_addr = InetAddress.getByAddress(ip_bytes);                                socket_at = new DatagramSocket(AT_PORT);        socket_at.setSoTimeout(3000);                Test1 ardrone = new Test1("ARDrone", args, inet_addr);                while(true) {           Thread.sleep(30);        }     }         public static void send(String msg) throws Exception {        byte[] bytes = (msg + "\r").getBytes();        DatagramPacket ds = new DatagramPacket(bytes, bytes.length, inet_addr, AT_PORT);                socket_at.send(ds);     }               public static synchronized int get_seq() {        return seq++;     }             public void keyTyped(KeyEvent e) {         ;     }         public void keyPressed(KeyEvent e) {         int keyCode = e.getKeyCode();         try {             control(keyCode);         } catch (Exception ex) {             ex.printStackTrace();         }     }         public void keyReleased(KeyEvent e) {         ;         //if (keyCode >= KeyEvent.VK_1 && keyCode <= KeyEvent.VK_9) speed = (float)0.1; //Reset speed             }         //Control AR.Drone via AT commands per key code     public void control(int keyCode) throws Exception {               switch (keyCode) {            case KeyEvent.VK_SPACE:               // Операция  Не равно "= !"               space_bar = !space_bar;              if (space_bar) {                    System.out.println("Takeoff");                   send("AT*CONFIG=" + get_seq() + ",\"leds:leds_anim\",\"3,1073741824,2\"");                    //send("AT*REF=" + get_seq() + ",290718208");          } else {                   System.out.println("Landing");                   send("AT*CONFIG=" + get_seq() + ",\"leds:leds_anim\",\"5,1073741824,2\"");                    //send("AT*REF=" + get_seq() + ",290717696");           }               break;                                         default:               break;        }             }           }

Алгоритмы поведения drone

Логика поведения drone

1. Таймер watchdog
2. Две секунды
3. Состояние emergency (Авария)
4. Алгоритмы стабилизации полета при ветре
5. Алгоритм полета в режиме hovering
6. Команды взлета и посадки.
7. Работа с конфигурационными данными.


Интересно, что будет если дать команду "Посадка", а потом не дожидаясь посадки дать команду "Газ".

По моему в памяти вертолета есть несколько полей состояний которые определяют его поведение. С помощью команд мы изменяем значения нужных битов в этих полях.

Несколько раз наблюдал, что в случае нештатной посадки, например emergency, при взлете вертолет сразу резко начинает маневрировать непонятно. То есть вертолет помнит прошлые команды клиента. Если их не отменить, он продолжает их выполнять.

Еще одно замечание про порядковый номер команды. В памяти у вертолета есть поле в котором хранится значение номера последней выполненной команды. Чтобы не выполнять команды которые переданы вертолету раньше чем последняя исполненная.

То есть программа program.elf приняв команду сравнивает ее номер с номером хранящемся в поле счетчика команд, если номер пришедшей команды больше номера хранящегося в счетчике команд, то команда будет выполнена

Есть исключение из этого правила, если номер команды "единица", то значение в счетчике команд сбрасывается и номер считается правильным, то есть команда будет выполнена вертолетом

То есть в качестве номера команды можно всегда использовать число один, это не приведет к ошибке, то есть все команды будут с таким номером будут выполнены.

[внешняя ссылка]

Сообщение от
Stéphane Piskorski
A.R.Drone Software Engineer

Управление AR.Drone с помощью жестов

[внешняя ссылка]