A partir deste Post começaremos uma série ensinando como criar um controle para o Ar.drone com interface natural, através do kinect. Ela será divida em 3 partes, na primeira (e a atual fase) trataremos de como fazer a comunicação do computador com Ar.Drone através do C++, e os comandos necessários para movê-lo. Na segunda trataremos da forma de recebimento destes comandos pelo Kinect. Na terceira e última explicaremos do modo de recebimento de dados da câmera e sensores do Ar.drone.
Existem outros artigos no site falando sobre o AR.Drone, neles tivemos uma visão geral sobre ele (aqui), um artigo específico sobre a comunicação (aqui) e uma série falando sobre voo autônomo com JavaScrip (parte1, parte2, parte3).
Comunicação AR.Drone
Como dito acima já temos um artigo tratando sobre este assunto (aqui), porem, a fim de tornar o aprendizado mais dinâmico, farei um resumo.
Basicamente ao ser ligado o AR.Drone inicia uma rede wifi, com o nome ardrone_xxx, ao se conectar a ela podemos trocar dados com o drone, enviando comandos de controle e recebendo dados dos sensores e das câmeras.
A comunicação e feita através de portas UDP, separadas por funcionalidades, são elas a porta 5556, que recebe comandos AT, como os comandos de controle do drone, a porta 5554, responsável por enviar dados dos sensores dos drone, a porta 5555, a qual envia o stream de video, e a porta 5559, responsável por enviar dados de emergência, ela e conhecida como control port.
Neste artigo detalharei sobre como fazemos uma comunicação com a porta 5556 utilizando o c++, para assim criamos um controle para o drone, na parte 3 desta série vamos focar em como recebemos dados, tanto da porta 5555, recebendo as imagens da camera, como da porta 5554, com os dados dos sensores.
Comandos AT
É um grupo de comandos reconhecidos pelo AR.Drone, eles podem ser enviados pela porta 5556 em formatos de string, a partir deles o drone reconhece comandos como movimentos, configurações, controles dos leds e outros.
Ele e reconhecido através de uma string (em ASCII de 8bits) com no maximo 1024 bits, que se inicia com “AT*” e finaliza com o delimitador <LF> (quebra de linha ou “/r” em C++), sua sintaxe pode ser resumida na seguinte expressão:
“AT*”+COMANDO+“=”+NºSerie+ARGUMENTOS+“/r”.
Como por exemplo “AT*PCMD=21625,1,0,0,0,0/r”;
Existem 8 tipos diferentes de comandos, que são diferenciados pela variavel comando, cada um recebe diferentes argumentos e tem diferentes funções, a tabela abaixo descreve todos os comandos. Já a variável NºSerie serve como sicronizador, sendo um valor que aumenta constamente a cada comando enviado, caso contrário o comando sera rejeitado.
| Comando AT | Argumentos | Descrição |
| AT*REF | input | Decolagem/Pouso/Parada de Emergência |
| AT*PCMD | flag, roll, pitch, gaz, yaw | Movimentos do Drone |
| AT*PCMD_MAG | flag, roll, pitch, gaz, yaw, psi, psi accuracy | Movimentos do Drone (com comando absoluto) |
| AT*FTRIM | – | Calibração (quando drone esta no chão) |
| AT*CONFIG | key, value | Configuração AR.Drone 2.0 |
| AT*CONFIG_IDS | session, user, application ids | Identificadores para comandos AT*CONFIG |
| AT*COMWDG | – | Reset comunicação Watchdog |
| AT*CALIB | device number | Calibração para o magnetrômetro |
| AT*ANIM | animação e duração | Sequência de animação de voo do drone |
Como nosso propósito é a criação de controle para o AR.Drone, focaremos nos comandos AT*REF, AT*FTRIM e AT*PCMD.
Controlando o AR.Drone
Para que possamos criar um controle para o drone e necessário entendermos melhor os 3 comandos destacados acima.
AT*FTRIM: Este é um simples comando de calibração, a partir dele o Drone ira se ajustar no plano horizontal, ele deve ser executado sempre antes da decolagem, enquanto o AR.Drone ainda estiver no chão, caso contrário ele poderá sofre desestabilização ao decolar ou se mover.
Sintaxe: “AT*FTRIM=%d/r”
- Argumento 1: nº sequencia;
AT*REF: Responsável por comandos básicos do Drone, dentre eles dois são de suma importância para seu controle, a decolagem e a aterrizagem descritas a seguir;
Sintaxe: “AT*REF=%d,%d/r”
- Argumento 1: nº sequencia;
- Argumento 2: valor inteiro de 32 bit;
Comandos importantes:
Decolagem: “AT*REF=1,290718208/r”
Aterrizagem: “AT*REF=2,290717696/r”
AT*PCMD: É o principal comando para o controle do drone, com ele serão passados as movimentos que a serem feitos, ele recebe como argumentos os movimentos feitos pelo drone em seus eixos,roll,pitch,gaz,yaw (ele estão descritos aqui), para facilitar sua compreção iremos descrevê-los também em sentidos de referencia do drone.
Sintaxe: “AT*PCMD=%d,%d,%d,%d,%d,%d/r”
- Argumento 1: nº sequencia;
- Argumento 2: Flag do Drone;
- Argumento 3: Roll (positivos move para direita, negativos esquerta);
- Argumento 4: Pitch (positivos move para trás, negativos frente);
- Argumento 5: Gaz (positivos move para cima, negativos baixo);
- Argumento 6: Yaw (positivos rotaciona direita, negativos esquerda).
Seus valor são recebidos como variáveis inteiras, mas são referentes a floats entre 1 e -1, que significam a potencia com que o drone ira se mover, abaixo tempos alguns dos valores convertidos:
- -1082130432 = -1;
- -1086324736 = -0.75;
- -1090519040 = -0.5;
- -1098907648 = -0.25;
- 0
- 1048576000 = 0.25;
- 1056964608 = 0.5;
- 1061158912 = 0.75;
- 1065353216 = 1;
A função de flag é decidir se o drone estará em modo hover (piloto automático) ou se ele será controlado pelo usuário, sendo assim:
- Bit 0: drone em modo hover
- Bit 1: drone em modo controle
Criando comunicação com C++
Para criar a comunicação utilizaremos a biblioteca QudpSocket parte das bibliotecas oferecidas pelo qt; Nela podemos iniciar a comunicação com drone com o seguinte comando:
[code language=”c”]
QUdpSocket *socket; //socket que será responsável pela envio de dados
QHostAddress ip; //Ip do drone
ip= "192.168.1.1";
socket = new QudpSocket(this);//Iniciando socket
socket->bind(ip ,5556); //vinculando o ip do drone e porta de envio de comando ao socket
connect(socket, SIGNAL(readyRead()), this, SLOT(readyRead()));
[/code]
Movimentando Drone
Com a conexão iniciada a movimentação do drone será simplesmente o envio de strings através do socket, as funções abaixo exemplificam este envio para 3 comandos, o de decolagem, subida do drone, e de movimentar para esquerda, é importante lembra quem sempre antes da decolagem e necessário fazer o comando AT*FTRIM sem ele o drone pode perder estabilidade ao decolar ou mover:
[code language=”c”]
void ConexaoDrone::esquerda()
{
QByteArray Data;
//Estabizando Drone
Data.append("AT*FTRIM=1\r"); //comando para ligar
conexDrone->socket->writeDatagram(Data,conexDrone->ip,5556);
usleep(100);
//Ligando Drone o drone
qDebug()<<"liga";
Data.append("AT*REF=2,290718208\r"); //comando para ligar
conexDrone->socket->writeDatagram(Data,conexDrone->ip,5556);
contador=1;
}
void ConexaoDrone::esquerda()
{
qDebug()<<"esquerda";
contador++;
QByteArray Data;
std::ostringstream comando;
comando << "AT*PCMD="<<contador<<",1,-1119040307,0,0,0/r";
//comando << "AT*PCMD="<<contador<<",1,N1102263091,0,0,0/r";
Data.append(comando.str().c_str());
socket->writeDatagram(Data,ip,5556);
}
void ConexaoDrone::cima()
{
qDebug()<<"cima";
contador++;
QByteArray Data;
std::ostringstream comando;
comando << "AT*PCMD="<<contador<<",1,0,0,1036831949,0\r";
//comando << "AT*PCMD="<<contador<<",1,0,0,1045220557,0\r";
Data.append(comando.str().c_str());
socket->writeDatagram(Data,ip,5556);
}
[/code]
Utilizando Threads
Para manter a conexão com o AR.Drone ativa precisamos enviar dados com intervalos de no máximo 2 segundos, porem o ideal e o envio de 30 comandos por segundo. Tanto processamento pode atrapalhar as outra funcionalidades de nosso programa, para que isto não aconteça podemos utilizar de threads serem responsável por enviar dados no intervalo de 0.03s.
Para isto podemos usar a biblioteca pthread.h, e criar uma thread que será responsável por chamar uma função de movimento a cada 0.03s.
Isto pode ser feito com os seguintes comandos:
[code language=”c”]
void ConexaoDrone::liga()
{
//inicia a thread
pthread_t threadMovimenta;
pthread_create(&threadMovimenta, NULL, Movimentar, NULL);
}
void *ConexaoDrone::Movimentar(void*){ //Consumidor
while(1){
conexDrone->contador++;
//Inicio seção critica
while(semaforo==0);
semaforo = 0;
valorMovimento = conexDrone->movimento;
semaforo = 1;
//Fim seçao critica
saida = conexDrone->MudarMovimento(valorMovimento);
if (saida == 0)
break;
usleep(30); //esperar para enviar proximo sinal para o drone
}
}
int ConexaoDrone::MudarMovimento(int valorMovimento){
switch(valorMovimento){
case 10:
desliga();
return 0;
break;
case 0:
centro();
return 1;
break;
case 1:
direita();
return 1;
break;
case 2:
esquerda();
return 1;
break;
case 3:
cima();
return 1;
break;
case 4:
baixo();
return 1;
break;
default:
qDebug()<<"Comando Não reconhecido";
return 1;
break;
}
}
[/code]
Como são determinados os valores inteiros equivalentes aos floats? Fiquei um pouco confuso nessa parte.