Библиотека радиолюбителя

Блок управления квадрокоптером ArDrone 2.0 на модуле ESP8266

Размеры, цена и наличие WiFi позволяют сделать бюджетный блок управления квадрокоптером ArDrone 2.0 на модуле ESP8266 (цены на AliExpress, Gearbest). Для управления будем использовать Модуль GY-521 на микросхеме MPU6050 (гироскоп, акселерометр). 

Parrot AR.Drone – это радиоуправляемый квадрокоптер, то есть вертолет с четырьмя несущими винтами, размещенных на выносных диагональных балках. Сам AR.Drone работает под управлением операционной системы Linux, а в качестве пульта ДУ к квадрокоптеру может выступать практически любой сенсорный смартфон и планшет на Android или iOS. Дистанция устойчивого управления по Wi-Fi – от 25 до 100 метров и зависит от помещения и погодных условий, если полеты происходят на улице.

При включении AR.Drone создает точку доступа SSIS «ardrone_XX_XX». Подключение без пароля.
Попробуем подключиться к точке доступа Ar.Dron-а с помощью AT-команд
Подключим плату ESP8266 к com-порту компьютера через переходник UART —> USB
подаем питание 3,3 В.

Откроем Arduino IDE, монитор последовательного порта и будем отправлять на плату ESP AT-команды (квадрокоптер должен быть включен) 

Связь с AR.Drone осуществляется с помощью AT команд. 
Команды отправляются на AR.Drone как UDP или TCP — пакеты; 
Один пакет UDP должен содержать, по крайней мере, одну полную команду или более; 
В случае, если пакет содержит более одной команды, то для разделения команд используется символ 0x0A.

 Строки кодируются в виде 8-битовых символов ASCII; 
Максимальная длина команды составляет 1024 символов; 
Между командами задержка 30 мс. Команда состоит из AT * [имя команды] = [порядковый номер команды в виде строки] [, аргумент1, аргумент 2 …] Список основных AT-команд для управления AR.Drone:

  • AT*REF — используется для взлета, посадки, сброса и аварийной остановки;
  • AT*PCMD — эта команда используется для управления движением AR.Drone;
  • AT*FTRIM — на горизонтальной плоскости;
  • AT*CONFIG — настройка параметров AR.Drone;
  • AT*LED — устанавливает LED-анимации на AR.Drone;
  • AT*ANIM — установка полетной анимации на AR.Drone.
  • AT*COMWDG — команда сброса watchdog — посылаем ее постоянно в квадрокоптер.

Для связи используются следующие порты:

  •  Порт 5556 — UDP — отправка команд на AR.Drone;
  •  Порт 5554 — UDP — получение пакетов данных от AR.Drone;
  •  Порт 5555 — Ответить поток видео пакеты из AR.Drone;
  •  Порт 5559 — TCP — пакеты для критически важных данных, которые не могут быть потеряны, как правило, для конфигурации.

Клиент отключается от UDP порта после задержки в 2 секунды после отправки последней команды!!! — поэтому необходимо постоянно посылать команды, при отсутствии необходимых — AT*COMWDG.

Рассмотрим получение навигационных данных от ARDrone (Порт 5554 — UDP).
Пакет навигационных данных в режиме demo имеет длину 500 байт. В случае если что то идет не так, то drone может присылать пакет длиной 32 и 24 байта. Если пакет имеет длину 24 байта это означает что порт 5554 находится в режиме BOOTSTRAP и необходимо заново подсоединится к порту чтобы перевести его режим Demo 
ARDrone может передавать клиенту навигационные данные в двух формах:

  • cокращенной (или demo), размер 500 байт; 
  • полной.

Чтобы получать demo-данные, надо отправить на порт 5554 сначала четыре байта 0x01, 0x00, 0x00, 0x00, а затем на порт 5556 команду 

AT*CONFIG=»+(seq++)+»,»general:navdata_demo»,»TRUE» 
где seq — порядковый номер команды.

Структура пакета навигационных данных. В начале пакета присутствуют 4 именованных величины: 

  • заголовок пакета 32 бита:
  • флаги состояния вертолета 32 бита;
  • порядковый номер последней команды переданной вертолету клиентом 32 бита;
  • vision flag 32 бита. 

Далее — Заголовок опции navdata: 20-23;

 Опция navdata имеет следующие поля:

  • BATTERY = 24; заряд батареи в процентах; 
  • PITCH = 28; угол наклона по продольной оси;
  • ROLL = 32; угол наклона относительно поперечной оси; 
  • YAW = 36; угол поворота относительно вертикальной оси; 
  • ALTITUDE = 40; высота; 
  • VX = 44; скорость по оси Х;
  • VY = 48; скорость по оси Y; 
  • VZ = 52; скорость по оси Z.
  • На время отладки подсоединим к плате ESP8266 дисплей Nokia 5110

Подсоединим к модулю ESP8266 дисплей Nokia5110 и будем выводить на него и в монитор последовательного порта часть навигационных данных.

Содержимое скетча 

 #include <SPI.h>  #include <Adafruit_GFX.h>  #include <Adafruit_PCD8544.h>  // ESP8266 Software SPI (slower updates, more flexible pin options):  // pin 14 - Serial clock out (SCLK)  // pin 13 - Serial data out (DIN)  // pin 12 - Data/Command select (D/C)  // pin 15 - LCD chip select (CS)  // pin 4 - LCD reset (RST)  Adafruit_PCD8544 display = Adafruit_PCD8544(14, 13, 12, 15, 4);    #include <ESP8266WiFi.h>  #include <WiFiClient.h>  #include <IPAddress.h>  #include <WiFiUdp.h>  #include <stdio.h>  #include <inttypes.h>      const char* ssid = "ardrone2_060602";  const int navPort = 5554;  const int atPort = 5556;  const IPAddress drone(192, 168, 1, 1);    byte pos;  unsigned int sequence;  unsigned int lastNav;  unsigned int lastPacket;  WiFiUDP Udp;  WiFiUDP AT;  String sendBuffer;  char incoming[1024];      void setup(void) {   Serial.begin(115200);   Serial.println("");   Serial.println("Starting");   // initialize the LCD   display.begin();   display.setContrast(50);   display.display(); // show splashscreen   delay(2000);   display.clearDisplay();  // clears the screen and buffer   display.setTextSize(1);   display.setTextColor(BLACK);     // Turn on the blacklight and print a message.   display.setCursor(0,0);   display.print("WiFi connect ...");   display.display();      pos = 0;   sequence = 1;   // Connect to WiFi network   WiFi.mode(WIFI_STA);   WiFi.begin(ssid);       // Wait for connection   while (WiFi.status() != WL_CONNECTED) {     delay(200);   }   Serial.println("Connected!");   Serial.println(WiFi.localIP());   display.clearDisplay();   display.setCursor(0,0);   display.print("OK");   delay(3000);   display.setCursor(0,20);   display.print(WiFi.localIP());   display.display();   //   pinMode(pinButton,INPUT);   //  Udp.begin(navPort); //Open port for navdata   Udp.flush();   AT.begin(atPort);   AT.flush();   String configg = "AT*CONFIG=";   configg += String(sequence);   configg += ","general:navdata_demo","TRUE"r";   while(Udp.parsePacket() == 0) {     delay(10);     Udp.beginPacket(drone, navPort);     Udp.write(0x01);     Udp.endPacket();     delay(10);     sendPacket(configg);   }   Serial.println("Starting main loop");   //delay(3000);      }    void loop(void) {   if(Udp.parsePacket()) {     int len = Udp.read(incoming, 1024);     Serial.print("length=");Serial.println(len);     if (len < 30) return;     incoming[len] = 0;     Serial.print("header=");printParamData(0,4);Serial.println();     Serial.print("state=");printParamData(4,4);Serial.println();     Serial.print("pitch=");printParamData(28,1);Serial.println();     Serial.print("roll=");printParamData(32,1);Serial.println();   Serial.print("yaw=");printParamData(36,1);Serial.println();     Serial.print("altitude=");printParamData(40,1);Serial.println();     Serial.print("battery=");printParamData(24,1);Serial.println();   Serial.print("vx=");printParamData(44,1);Serial.println();     Serial.print("vy=");printParamData(52,1);Serial.println();   Serial.println("********************************************");     // печать параметров на дисплей     printdatalcd();   }   // отправка пакета для поддержания соединения   if(millis() - lastPacket > 1000) {     String tmr = "AT*COMWDG=";     tmr += String(sequence);     sendPacket(tmr);     Serial.print("send=");Serial.println(tmr);   }  }  // отправка в порт 5554  void sendPacket(String &string) {   char sendChar[string.length()+1];   string.toCharArray(sendChar, string.length()+1);   sendChar[string.length()] = 'r';   AT.beginPacket(drone, atPort);   AT.write(sendChar);   AT.endPacket();   sequence++;   lastPacket = millis();  }  // печать данных в последовательный порт  void printParamData(int offset,int count) {     for(int i=count;i>0;i--) {     Serial.print(incoming[offset+i-1],HEX);Serial.print(" ");     }  }  // данные на экране lcd  void printdatalcd() {     // status     display.clearDisplay();   display.setCursor(0,0);     display.print(incoming[4],HEX);display.print(" ");     display.print(incoming[5],HEX);display.print(" ");     display.print(incoming[6],HEX);display.print(" ");     display.print(incoming[7],HEX);display.print(" ");     // battery     display.setCursor(0,20);     display.print(incoming[24],DEC);     display.print("%");     // altitude h     display.setCursor(0,40);     display.print(incoming[40],HEX);display.print(" ");     // vx     display.print(incoming[40],HEX);display.print(" ");     // vy     display.print(incoming[40],HEX);display.print(" ");     // vz     display.print(incoming[40],HEX);display.print(" ");   display.display();   }

Загружаем (скетч ardrone_esp8266_01.ino), и наблюдаем вывод навигационных данных в последовательный порт и на экран дисплея.

Отправка команд взлета и посадки   

Теперь добавим в наш проект взлет и посадку квадрокоптера командами с пульта. Для взлета необходимо отправить команду

AT*REF=[Sequence number ], 290718208<LF>

Для посадки

AT*REF=[Sequence number ], 290717696<LF>

Перед взлетом необходимо отправить команду для горизонтальной калибровки, иначе ArDrone не сможет стабилизироваться при полете.

AT * FTRIM=[Sequence number ]<LF>

Добавляем к нашей схеме кнопку для взлета и посадки.

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

 int pinButton=5;  int lastButtons1=0;  int currentButtons1=0;  boolean onLand=true;  Процедуру борьбы с дребезгом:  // проверка на дребезг  int debounce(int last,int pin1)   {   int current = digitalRead(pin1);  // Считать состояние кнопки   if (last != current)   // если изменилось...     {     delay(5);   // ждем 5мс     current = digitalRead(pin1);  // считываем состояние кнопки     return current;   // возвращаем состояние кнопки     }   }

И обработку нажатий кнопки: 

 // проверка нажатия кнопки    currentButtons1 = debounce(lastButtons1, pinButton);    if (lastButtons1 == 0 && currentButtons1 == 1) // если нажатие...     {     // изменить состояние реле     onLand=!onLand;     // вывести в порт     Serial.print("onLand=");Serial.println(onLand);     if(onLand==false) { // takeoff     String tmr="AT*FTRIM=";     tmr += String(sequence);     sendPacket(tmr);     delay(50);     tmr = "AT*REF=";     tmr += String(sequence);     tmr += ",290718208";     sendPacket(tmr);     }     else { // landing     String tmr = "AT*REF=";     tmr += String(sequence);     tmr += ",290717696";     sendPacket(tmr);     }    }    lastButtons1 = currentButtons1;   

Загружаем скетч ardrone_esp8266_02.ino () на плату ESP8266, включаем  квадрокоптер ArDrone 2.0 и проверяем работу кнопки. При нажатии – взлет, при следующем нажатии – посадка и т.д.

Подключение MPU6050 для управления Ardrone 2.0

Датчики определения положения в пространстве применяются для управления в квадрокоптерами. Микросхема MPU6050 содержит на борту как акселерометр, так и гироскоп, а помимо этого еще и температурный сенсор. MPU6050 является главным элементом модуля GY-531 (рис. 15.44). Помимо этой микросхемы на плате модуля расположена необходимая обвязка MPU6050, в том числе подтягивающие резисторы интерфейса IC, а также стабилизатор напряжения на 3,3 вольта с малым падением напряжения (при питании уже в 3,3 вольта на выходе стабилизатора будет 3 ровно вольта) с фильтрующими конденсаторами.

Подключение к микроконтроллеру по протоколу I2C. 

Использование акселерометра и гироскопа позволяет определить отклонение по осям x и y, и отклонение «превратить» в команды для движения квадрокоптера по соответствующим осям. Перевод показаний, получаемых с датчика в угол отклонения: 

 uint8_t* data = i2cRead(0x3B,14);  accX = ((data[0] << 8) | data[1]);   accY = ((data[2] << 8) | data[3]);   accZ = ((data[4] << 8) | data[5]);  //tempRaw = ((data[6] << 8) | data[7]);  gyroX = ((data[8] << 8) | data[9]);   gyroY = ((data[10] << 8) | data[11]);   gyroZ = ((data[12] << 8) | data[13]);     /* Calculate the angls based on the different sensors and algorithm */   accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;   accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;   double gyroXrate = (double)gyroX/131.0;   double gyroYrate = -((double)gyroY/131.0);   // Calculate gyro angle without any filter   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000);  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);  И значения, получаемые при использовании комплиментарного фильтра и фильтра Кальмана:  // значения при применении комплиментарного фильтра  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle); // значения при применении фильтра Кальмана  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);

Команда, которую необходимо напрвлять ArDrone для управления полетом

AT*REF=[Sequence number ],[Flag bit-field],[Roll],[Pitch],[Gaz],[Yaw]<LF>

Значения Roll и Pitch в интервале -1 до 1 берем из таблицы const int floats[], индекс соответствует углу отклонения, вычисляемому из данных датчика MU6050.

Загружаем скетч ardrone_esp8266_03.ino его на плату ESP8266, включаем  квадрокоптер ArDrone 2.0 и проверяем работу пульта.

И видео работы