Размеры, цена и наличие 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 может передавать клиенту навигационные данные в 2-х формах:
- 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, в том числе подтягивающие резисторы интерфейса I2C, а также стабилизатор напряжения на 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 и проверяем работу пульта.
И видео работы