1. はじめに

ドローン「Tello」を「M5StickC」でコントロールするスケッチを作ってみました。

２. 環境

「Tello」と「M5StickC」間は、WiFi無線、UDPプロトコルで通信を行います。
Telloのコマンドは公式のTello SDKを使用しました。
https://www.ryzerobotics.com/jp/tello/downloads

２－１. 開発環境

・Arduino IDE 1.8.9

３. 操作方法

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

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

（３）上・下移動
　　　・後退に傾けながら、ボタン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からのメッセージ受信

５. スケッチでの注意点

（１）TelloのWiFi SSIDの修正
　　　8行目
　　　 const char* TELLO_SSID = "TELLO-XXXXXX"; // 自分のTelloのWi-Fi SSIDを入力
　　　自分のTelloの値に置き換える。
　　　Tello本体のバッテリー装着するところにSSIDが書かれたシールがあります。

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

６. 終わりに

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