Блок управления квадрокоптером 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, в том числе подтягивающие резисторы интерфейса I²C, а также стабилизатор напряжения на 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 и проверяем работу пульта.

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

Теги:

• ESP8266