TelloをM5StickCでコントロールするスケッチを作りました

# 1.   はじめに
ドローン<strong><font color="blue">「Tello」</font></strong>を<strong><font color="blue">「M5StickC」</font></strong>でコントロールするスケッチを作ってみました。


# ２.   環境
「Tello」と「M5StickC」間は、WiFi無線、UDPプロトコルで通信を行います。
Telloのコマンドは公式のTello SDKを使用しました。
https://www.ryzerobotics.com/jp/tello/downloads
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/59815/1313ffa0-6527-4bf4-f763-dff0468c6740.png)

## ２－１. 開発環境
・Arduino IDE 1.8.9

# ３.  操作方法

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/59815/99b9f41f-acf1-935a-817f-79c8bdb47d95.png)

<strong>（１）ボタン操作</strong>
　　　・ボタンA(真ん中)-----時計回りに45度回転
　　　・ボタンB(右側面)-----離陸と着陸トグル切替

<strong>（２）前進・後退・左・右移動</strong>
　　　・「M5StickC」を傾けた方向に「Tello」が移動します。

<strong>（３）上・下移動</strong>
　　　・後退に傾けながら、ボタンA(真ん中)を押す-----上昇
　　　・前進に傾けながら、ボタンA(真ん中)を押す-----下降

# ４.  スケッチ
```TelloController-M5StickC.ino
#define LOAD_FONT2
#define LOAD_FONT4
#include <M5StickC.h>

#include <WiFi.h>
#include <WiFiUdp.h>
// TELLOのSSID
const char* TELLO_SSID = "TELLO-XXXXX";  // 自分の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 = "";

float x;
float y;
//
char msgx[6];
char msgy[6];

float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
//float accZ = 0.0F;
float accX_sum = 0.0F;
float accY_sum = 0.0F;
float accX_diff = 0.0F;
float accY_diff = 0.0F;
int count;

bool if_land = true;

//
void setup() {
  M5.begin();
  M5.IMU.Init();
  //タイトル
  M5.Lcd.fillRect(0,0,80,20,TFT_BLUE);
  M5.Lcd.drawCentreString("Tello",40,2,1);
  M5.Lcd.drawCentreString("Controller",40,10,1);
  //方向文字背景
  M5.Lcd.fillTriangle(20,50,40,30,60,50,TFT_RED);
  M5.Lcd.fillTriangle(20,50,40,70,60,50,TFT_RED);
  //方向文字
  M5.Lcd.setTextColor(TFT_BLACK,TFT_YELLOW);
  M5.Lcd.drawCentreString("F",40,20,4);
  M5.Lcd.drawCentreString("B",40,60,4);
  M5.Lcd.drawCentreString("L",0,40,4);
  M5.Lcd.drawCentreString("R",70,40,4);
  //---Xの表示
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawCentreString("accX: ",20,90,1);
  sprintf(msgx,"%-2.2f",x);
  M5.Lcd.drawCentreString(msgx,60,90,1);
  //---Y値の表示
  M5.Lcd.drawCentreString("accY: ",20,100,1);
  sprintf(msgy,"%-2.2f",y);
  M5.Lcd.drawCentreString(msgy,60,100,1);
  //  
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawString("BtnA:Roll",2,110,1);
  M5.Lcd.drawString("BtnA + F:Down",2,119,1);
  M5.Lcd.drawString("BtnA + B:Up",2,128,1);
  //---メッセージの文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawString("Landing...",2,140,1);
  //補正値の値取得
  //ダミーで加速度取得
  for (count = 1; count <= 10; count = count + 1){
    delay(200);
    M5.IMU.getAccelData(&accX,&accY,&accZ);
  }
  //加速度を10回取得し、補正値を取得
  for (count = 1; count <= 10; count = count + 1){
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    //Serial.println(accX);
    //Serial.println(accY);
    //Serial.println("-------");
    delay(200);
    accX_sum = accX_sum + accX;
    accY_sum = accY_sum + accY;
  }
  //補正値
  accX_diff = accX_sum / 10;
  accY_diff = accY_sum / 10;
  //
  //初期設定
  Wire.begin();
  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("send commend");
  tello_command_exec("command");
  delay(500);
}

void loop() {
  // put your main code here, to run repeatedly:
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  x = accX - accX_diff;
  y = accY - accY_diff;  
  //---Xの表示
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawCentreString("accX: ",20,90,1);
  sprintf(msgx,"%-2.2f",x);
  M5.Lcd.drawCentreString("        ",60,90,1);
  M5.Lcd.drawCentreString(msgx,60,90,1);
  //---Y値の表示
  M5.Lcd.drawCentreString("accY: ",20,100,1);
  sprintf(msgy,"%-2.2f",y);
  M5.Lcd.drawCentreString("        ",60,90,1);
  M5.Lcd.drawCentreString(msgy,60,100,1);
  
  //ボタンB処理
  //離陸
  if(M5.BtnB.wasPressed()) {
    print_msg("TAKE OFF"); 
    tello_command_exec("takeoff");
    delay(500);  
  }
  //着陸
  if(M5.BtnB.pressedFor(300)) {
    print_msg("LAND"); 
    tello_command_exec("land");
    delay(500);  
  }


      //ボタンA処理    
    if(M5.BtnA.wasPressed()) {
      //着陸
      if (fabs(y)> 0.5){
        //上昇
        if (y > 0){
          tello_command_exec("up 50");
        }
        //下降
        if (y < 0){
          print_msg("DOWN");
          tello_command_exec("down 50");
        }
      }else{
        //回転
        print_msg("CW");
        tello_command_exec("cw 45");
      }
    }

 
  //tello移動
  if (fabs(x)> 0.5){
      //左移動
      if (x > 0){
        print_msg("LEFT");
        tello_command_exec("left 50");
      }
      //右移動
      if (x < 0){
        print_msg("RIGHT");
        tello_command_exec("right 50");
      }
  }
  if (fabs(y)> 0.5){
      //後退
      if (y > 0){
        print_msg("BACK");
        tello_command_exec("back 50");
      }
      //前進
      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("              ",2,140,1);
  M5.Lcd.drawString(status_msg,2,140,1);
  status_msg="";
}

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;
    }
  }
  return (char*) packetBuffer;
}

```

## ４－１.  変数

| 型          | 変数名            | コメント                                |
| ----------- | ----------------- | --------------------------------------- |
| 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メッセージ受信バッファー  |
| String      | message           | Telloからの受信メッセージ               |
| float       | accX              | 加速度x                                 |
| float       | accY              | 加速度y                                 |
| float       | accX_sum          | 加速度xの10回合計(平均値を求める)         |
| float       | accY_sum          | 加速度yの10回合計(平均値を求める)         |
| float       | accX_sum          | 加速度xの補正値                          |
| float       | accY_sum          | 加速度yの補正値                         |
| char        | msgx[6]           | 加速度x文字列                           |
| char        | msgy[6]           | 加速度y文字列                           |
| bool        | if_land           | 離陸、着陸の状況フラグ(初期値:true　着陸)  |


## ４－２.  ユーザ関数

| ユーザ関数                                     | コメント                                 |
| ---------------------------------------------- | ---------------------------------------- |
| void print_msg(String   status_msg)            | 画面メッセージエリアへ状況メッセージ表示 |
| void   tello_command_exec(char* tello_command) | Telloへメッセージ送信＆コマンド実行      |
| String   listenMessage()                       | Telloからのメッセージ受信                |

# ５.  スケッチでの注意点
<strong>（１）TelloのWiFi SSIDの修正</strong>
　　　<font color="blue">8行目
　　　  const char* TELLO_SSID = "TELLO-XXXXXX";  // 自分のTelloのWi-Fi SSIDを入力</font>
　　　自分のTelloの値に置き換える。
　　　Tello本体のバッテリー装着するところにSSIDが書かれたシールがあります。

<strong>（２）加速度X,Y基準値の調整</strong>
　　　<font color="blue">142行目
　　　　if (fabs(y)> 0.5){
　　　161行目
　　　  　if (fabs(x)> 0.5){{
　　　173行目
　　　  　if (fabs(y)> 0.5){</font></font>
　　　左右前後の移動がうまくいかないときはこの値を調整してください。

# ６.  終わりに
加速度X,Yの補正値を取得するために、10回ダミーで加速度を読み取ったのちに、10回読み取りを行なってからその平均値を補正値としました。
位置情報は四元数を使えばより正確になると思いますが、今回は簡易的な方法で行いました。