1. 소개

드론 「Tello」를 M5Stack로 컨트롤하는 스케치를 만들어 보았습니다.
M5Stack은 BASIC이 아니라 가속도, 자이로, 자기를 계측 가능한 9축 센서, MPU9250을 탑재한 모델의 「M5Stack Gray」를 사용했습니다.

2. 환경

「Tello」와 「M5Stack Gray」사이는, WiFi 무선, UDP 프로토콜로 통신을 실시합니다.
Tello 명령은 공식 Tello SDK를 사용했습니다. 다소 어색한 동작이지만 참아주십시오.
htps //w w. ry 제로 보치 cs. 코 m / jp / 헉 / 드 w ぉ 아 ds

2-1. 개발 환경

· Arduino IDE 1.8.9

3. 조작 방법

(1) 버튼 조작
　　・버튼A(좌단)-----이륙
　　・버튼 B(가운데)---시계 방향으로 45도 회전
　　·버튼 C(오른쪽)-----착륙

(2) 전진·후퇴·좌·우 이동
　　・「M5Stack Gray」를 기울인 방향으로 「Tello」가 이동합니다.

(3) 플립
・플립하고 싶은 방향으로 기울이면서, 버튼 A(좌단)를 누른다.

(4)상·하 이동
　　・후퇴에 기울이면서, 버튼 C(좌단)를 누른다-----상승
　　・전진에 기울이면서, 버튼 C(좌단)를 누른다-----하강

4. 스케치

TelloController.ino

#define LOAD_FONT2
#define LOAD_FONT4
#include <M5Stack.h>
#include "utility/MPU9250.h"
#include <WiFi.h>
#include <WiFiUdp.h>
// TELLOのSSID
const char* TELLO_SSID = "TELLO-XXXXXX";  // 自分のTelloのWi-Fi SSIDを入力
// TELLOのIP
const char* TELLO_IP = "192.168.10.1";
// TELLO_PORT
const int PORT = 8889;
// UDPまわり
WiFiUDP Udp;
char packetBuffer[255];
String message = "";

MPU9250 IMU;

float x;
float y;
char msgx[6];
char msgy[6];
String status_msg;
void setup() {
  //M5Stackの初期設定
  M5.begin();
  //画面表示
  //---タイトル
  M5.Lcd.fillRect(0,0,320,30,TFT_BLUE);
  M5.Lcd.drawCentreString("Tello Controller",160,2,4);
  //---X, Yの表示
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawCentreString(" Accel-X : ",20,30,2);
  sprintf(msgx,"%-2.2f",x);
  M5.Lcd.drawCentreString(msgx,88,30,2);
  //---X, Y値の表示
  M5.Lcd.drawCentreString(" Accel-Y : ",240,30,2);
  sprintf(msgy,"%-2.2f",y);
  M5.Lcd.drawCentreString(msgy,294,30,2);
  //---ボタンエリア
  M5.Lcd.fillRect(0,217,320,20,TFT_LIGHTGREY);
  //---ボタン文字
  M5.Lcd.setTextColor(TFT_BLACK,TFT_YELLOW);
  M5.Lcd.drawCentreString(" TAKE OFF ",64,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_RED);
  M5.Lcd.drawCentreString(" LANDING  ",250,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_CYAN);
  M5.Lcd.drawCentreString(" ROTATION ",160,220,2);
  //---方向矢印
  M5.Lcd.fillTriangle(159,40,189,60,129,60,TFT_GREEN);
  M5.Lcd.fillTriangle(159,160,189,140,129,140,TFT_GREEN);
  M5.Lcd.fillTriangle(269,100,220,80,220,120,TFT_GREEN);
  M5.Lcd.fillTriangle(98,80,98,120,49,100,TFT_GREEN);
  //---方向の文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawCentreString("FORWARD",160,64,2);
  M5.Lcd.drawCentreString("BACK",160,120,2);
  M5.Lcd.drawCentreString("LEFT",120,92,2);
  M5.Lcd.drawCentreString("RIGHT",200,92,2);
  //---FLIP文字
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawCentreString("FLIP: ",60,140,1);
  M5.Lcd.drawCentreString("ButtonA + Direction",50,150,1);
  //---UP/DOWN文字
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawCentreString("UP/DOWN: ",280,140,1);
  M5.Lcd.drawCentreString("ButtonC + FORWARD",280,150,1);
  M5.Lcd.drawCentreString("Up/Down",300,160,1);
  //---メッセージ領域
  M5.Lcd.drawRoundRect(0,180,319,30,4,TFT_WHITE);
  //---メッセージのタイトル文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_DARKGREEN);
  M5.Lcd.drawCentreString("<Message>",38,170,1);
  //---メッセージの文字
  //M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  //M5.Lcd.drawString(msg,4,190,1);
  //初期設定
  //Wireライブラリを初期化
  Wire.begin();
  //MPU9250を初期化
  IMU.initMPU9250();
  //WiFi通信の開始
  WiFi.begin(TELLO_SSID, "");
  //WiFi接続　接続するまでループ
  while (WiFi.status() != WL_CONNECTED) {
        print_msg("Now, WiFi Connecting......");
        delay(500);
  }
  print_msg("WiFi Connected.");
  // UDP
  Udp.begin(PORT);
  //Telloへ”command”送信
  print_msg("sendMessage commend");
  tello_command_exec("command");  
}

void loop() {
  // put your main code here, to run repeatedly:
  //x,y値の取得と表示
  if (IMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){
    IMU.readAccelData(IMU.accelCount);
    IMU.getAres();
    x = IMU.accelCount[0] * IMU.aRes;
    y = IMU.accelCount[1] * IMU.aRes;
    sprintf(msgx,"%-2.2f",x);
    M5.Lcd.drawCentreString("      ",88,30,2);
    M5.Lcd.drawCentreString(msgx,88,30,2);
    sprintf(msgy,"%-2.2f",y);
    M5.Lcd.drawCentreString("      ",294,30,2);
    M5.Lcd.drawCentreString(msgy,294,30,2);
    print_msg("Operation Start!"); 

    //ボタンA処理
    //0.3は実測値から閾値を設定した
    if(M5.BtnA.wasPressed()) {
      //Telloフリップと離陸
      if (fabs(x)> 0.3){
          if (x > 0){
            //フリップ左
            print_msg("LEFT FLIP"); 
            tello_command_exec("flip l");
          }
          if (x < 0){
            //フリップ右
            print_msg("RIGHT FLIP"); 
            tello_command_exec("flip r");
          }
      }
      if (fabs(y)> 0.3){
          if (y > 0){
            //フリップ後
            print_msg("BACK FLIP"); 
            tello_command_exec("flip b");
          }
          if (y < 0){
            //フリップ前
            print_msg("FRONT FLIP"); 
            tello_command_exec("flip f");
          }
      }else{
        //離陸
        print_msg("TAKE OFF"); 
        tello_command_exec("takeoff");
      }    
    }

    //ボタンB処理
    //時計回り45度回転
    if(M5.BtnB.wasPressed()) {
        print_msg("CW");
        tello_command_exec("cw 45");
    }

    //ボタンC処理    
    if(M5.BtnC.wasPressed()) {
      //着陸
      if (fabs(y)> 0.3){
        //上昇50cm
        if (y > 0){
          tello_command_exec("up 50");
        }
        //下降50cm
        if (y < 0){
          print_msg("DOWN");
          tello_command_exec("down 50");
        }
      }else{
        //着陸
        print_msg("LAND");
        tello_command_exec("land");
      }
    }

    //tello移動
    if (fabs(x)> 0.3){
        //左移動50cm
        if (x > 0){
          print_msg("LFT");
          tello_command_exec("left 50");
        }
        //右移動50cm
        if (x < 0){
          print_msg("RIGHT");
          tello_command_exec("right 50");
        }
    }
    if (fabs(y)> 0.3){
        //後退50cm
        if (y > 0){
          print_msg("BACK");
          tello_command_exec("back 50");
        }
        //前進50cm
        if (y < 0){
          print_msg("FRONT");
          tello_command_exec("forward 50");
        }
    }
    delay(500);
  }
  M5.update();
}

/////////////////////////////
//      ユーザ関数定義       //
/////////////////////////////

// 画面メッセージエリアへ状況メッセージ表示
void print_msg(String status_msg){
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawString("                          ",4,190,1);
  M5.Lcd.drawString(status_msg,4,190,1);
  status_msg="";
}

// Telloへメッセージ送信＆コマンド実行
void tello_command_exec(char* tello_command){
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(tello_command);
  Udp.endPacket();
  message = listenMessage();
  delay(100);
}

// Telloからのメッセージ受信
String listenMessage() {
  int packetSize = Udp.parsePacket();
  if (packetSize) {
    IPAddress remoteIp = Udp.remoteIP();
    int len = Udp.read(packetBuffer, 255);
    if (len > 0) {
      packetBuffer[len] = 0;
    }
  }
  delay(100);
  return (char*) packetBuffer;
}

4-1. 변수

금형
변수 이름
코멘트


const char*
TELLO_SSID
자신의 Tello WiFi1SSID 설정

const char*
TELLO_IP
Tello IP 주소(기본값: 192.168.10.1)

const int
PORT
Tello 포트 번호(기본값: 8889)

WifiUDP
Udp
WifiUDP 클래스에서 Udp 인스턴스 생성

char
packetBuffer[255]
Tello에서 UDP 메시지 수신 버퍼

문자열
message
Tello에서 받은 메시지

MPU9250
IMU
MPU9250 클래스에서 IMU 인스턴스 생성

float
x
가속도 x

float
y
가속도 y

char
msgx[6]
가속도 x 문자열

char
msgy[6]
가속도 y 문자열

문자열
status_msg
상태 메시지

4-2. 사용자 함수

사용자 함수
코멘트


void print_msg (String status_msg)
화면 메시지 영역에 상태 메시지 표시

void tello_command_exec(char* tello_command)
Tello로 메시지 보내기 및 명령 실행

String listenMessage()
Tello로부터의 메시지 수신

5. 스케치의 주의점

(1) Tello의 WiFi SSID 수정
　　8행째
　　　 const char* TELLO_SSID = "TELLO-XXXXXX";//내 텔로의 Wi-Fi SSID 입력
　　 자신의 Tello의 값으로 바꿉니다.
　　Tello 본체의 배터리 장착하는 곳에 SSID가 쓰여진 씰이 있습니다.

(2) 가속도 X, Y 기준값의 조정
　　118행
　　　 if (fabs(x)> 0.3){
　　130행
　　　 if (fabs(y)> 0.3){
좌우 전후의 이동이 잘 되지 않을 때는 이 값을 조정해 주십시오.

6. 끝에

Tello 명령을 실행 한 후 delay (500)에서 500ms를 기다리지 만 값을 줄이면
부드럽게 움직일 수 있습니다.
바이너리 명령으로 실행하거나 M5StickC에서 스케치도 작성할 예정입니다.