M5stackからrcコマンドでTelloをコントロールする

(2019年3月16日追記)プログラムを変更しました．

#参考にしたページ
Ryze Tech. の Tello を M5stackで動かす方法としては，以下の神ページがあります．

　参考URL：TelloをM5Stack Grayでコントロールするスケッチを作りました

当該ページのプログラムを応用することで，
M5stack(あるいはESP32)を使って，Telloを様々に動かすことが可能になります．

なんと素晴らしいページなのでしょう！　改めてリスペクトと御礼を申し上げます(^^)

#はじめに

M5stackの加速度センサーを使ってTelloを操縦する方法は，上記のサイトを利用すれば可能です．
しかし，スマホやジョイパッドでコントロールする様な，なめらかな動きではありません．

本ページでは，当該プログラムを改造し，
　M5stackの傾け具合に応じて動的に操縦できるように
します．

具体的には，以下の動画の様に動かせるようになります．

M5Stackの参考プログラムをちょっと改造し、rcコマンドで直接動かしてます。

参考元: https://t.co/w3g36nx684 pic.twitter.com/Gy1yYFF7n3

— hsgucci404 (@hsgucci404) November 25, 2019

余談ですが，先日このTwitter動画が　「M5stack公式のブログ」　に載っていることを知りました．
M5stackの方が勝手に収集した動画を載せているようです．投稿なんかしていませんよ！(^^
なんか本家を差し置いて載ってしまってスミマセン(-_-;;;;滝汗

#概要

本家のページで使われているTelloの操作方法は，
Tello SDKのコマンド のなかでも，

forward back left right

といった，移動距離を指定するタイプのコマンドです．
このタイプのコマンドの特徴は，指定した距離ぶんの動作が完了し，停止(ホバリング)してから次のコマンドを待ちます．
すなわち，移動->停止->移動->停止 の繰り返しになるので，「リアルタイムなラジヘリ操縦」という気分にはなりません．

そこで，使用するコマンドを　Tello SDKのラジコン操作コマンド　に置き換えることで，なめらかな動きが可能になります．
　
　
※注意：
本記事では M5stackのプログラムをArduinoで開発する環境 の構築については割愛します．
それ専用に書かれた記事を検索してもらえれば，と思います．
筆者はLinuxで開発しているので，以下URLを参考にしました．
　Ubuntuで始めるM5stack

#スケッチ

M5stackに書き込むスケッチは，以下の様になります．

以下のコードをコピー＆ペーストするか，
ここ　を右クリックして[名前を付けて保存]機能でファイル保存してください．

TelloController_rc.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;

// 変更点(1) rcコマンド用文字列変数
char command_str[20];

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("CW/CCW_U/D",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);
  //---メッセージ領域
  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!"); 

    // 変更点(2) ボタンに応じた処理
    //ボタンA処理
    if(M5.BtnA.wasPressed()) {
      //離陸
      print_msg("TAKE OFF"); 
      tello_command_exec("takeoff");
    }

    //ボタンB処理
    if(M5.BtnB.isPressed()) {
      //---方向の文字 Bボタンを押しているとき
      M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
      M5.Lcd.drawCentreString("  DOWN  ",160,64,2);
      M5.Lcd.drawCentreString("  UP  ",160,120,2);
      M5.Lcd.drawCentreString(" CCW",120,92,2);
      M5.Lcd.drawCentreString("  CW ",200,92,2);
    } else {
      //---方向の文字 通常時
      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);
    }

    //ボタンC処理    
    if(M5.BtnC.wasPressed()) {
      //着陸
      print_msg("LAND");
      tello_command_exec("land");
    }

    // 変更点(3) 傾きに応じたコマンド送信
    if (fabs(x)> 0.3 || fabs(y)> 0.3){
        // 傾きx,yに応じてrcコマンドの文字列を作成
        
        //ボタンB処理
        if( M5.BtnB.isPressed() ) {
            sprintf(command_str,"rc 0 0 %d %d",int(y*100), int(-x*100) ); // 上昇下降と旋回
        } else {
            sprintf(command_str,"rc %d %d 0 0",int(-x*100), int(-y*100) ); // 左右移動と前後進
        }
        tello_command_exec(command_str);  // rcコマンドを送信
    } else {
        // 傾いていない時は停止命令を送信し続ける
        tello_command_exec("rc 0 0 0 0");
    }
    delay(50);  // 500ミリ秒のウェイトを50ミリ秒に減らした
  }
  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();

  // 変更点(3) Telloからの応答を無視
  //message = listenMessage();  // UDP受信の関数を走らせない

  delay(10);
}

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

##スケッチ解説

本家のプログラムから改変した点は４箇所です．

###スケッチ変更点(1)
１つ目の変更点では，rcコマンドの文字列を作成するために，グローバル変数として20文字ぶんの配列を用意しました．

グローバルな文字列変数を準備

// 変更点(1) rcコマンド用文字列変数
char command_str[20];

このcommand_strにsprintfを使って文字列を書き込みます．
実際に書き込む文字列は，rc 20 40 0 0の様になります．

###スケッチ変更点(２)
２つ目の変更点は，loop関数内でM5stackのボタン状態に応じた処理の部分です．

ボタン処理

    // 変更点(2) ボタンに応じた処理
    //ボタンA処理
    if(M5.BtnA.wasPressed()) {
      //離陸
      print_msg("TAKE OFF"); 
      tello_command_exec("takeoff");
    }

    //ボタンB処理
    if(M5.BtnB.isPressed()) {
      //---方向の文字 Bボタンを押しているとき
      M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
      M5.Lcd.drawCentreString("  DOWN  ",160,64,2);
      M5.Lcd.drawCentreString("  UP  ",160,120,2);
      M5.Lcd.drawCentreString(" CCW",120,92,2);
      M5.Lcd.drawCentreString("  CW ",200,92,2);
    } else {
      //---方向の文字 通常時
      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);
    }

    //ボタンC処理    
    if(M5.BtnC.wasPressed()) {
      //着陸
      print_msg("LAND");
      tello_command_exec("land");
    }

この場所には元々，

• 傾けてからボタンAを押すとフリップ
• ボタンBを押すと旋回
• 傾けてからボタンCを押すと上昇/下降

という機能もありましたが，
今回のプログラムでは，傾きに応じて常時rcコマンドを流し続けるので，upやflipなどの応答待ち系コマンドは一瞬で上書きされて効果が出ません．

そこで，

• ボタンAは離陸だけ
• ボタンBを押しながら傾けると左右旋回と上昇下降
• ボタンCは着陸だけ

としました．

###スケッチ変更点(３)
３つ目の変更点は，M5stackの傾き状態に応じてrcコマンドを作成する本体部分です．

rcコマンド送信部分

    // 変更点(3) 傾きに応じたコマンド送信
    if (fabs(x)> 0.3 || fabs(y)> 0.3){
        // 傾きx,yに応じてrcコマンドの文字列を作成
        
        //ボタンB処理
        if( M5.BtnB.isPressed() ) {
            sprintf(command_str,"rc 0 0 %d %d",int(y*100), int(-x*100) ); // 上昇下降と旋回
        } else {
            sprintf(command_str,"rc %d %d 0 0",int(-x*100), int(-y*100) ); // 左右移動と前後進
        }
        tello_command_exec(command_str);  // rcコマンドを送信
    } else {
        // 傾いていない時は停止命令を送信し続ける
        tello_command_exec("rc 0 0 0 0");
    }
    delay(50);  // 500ミリ秒のウェイトを50ミリ秒に減らした

sprintfでrcコマンド文字列を作成し，tello_command_exec関数で送信しています．
ボタンBの状態に応じて，前後進/左右移動と上昇下降/左右旋回を切り替えるようになっています．
isPressedとwasPressedで，ボタン押下中と押下後でタイミングが違います．

この場所には元々，以下のようなコードが書かれていました．

本家のプログラムでは

    //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);

本家ではforward 50とかleft 50とか送信していたコマンドを，
本記事ではrc 40 -30 0 0になる様に書き換えたわけです．

###スケッチ変更点(4)
４つ目の変更点は，UDPの受信部分の削除です．

UDPのlistenをコメントアウトした

  // 変更点(3) Telloからの応答を無視
  //message = listenMessage();  // UDP受信の関数を走らせない

Telloへ送信したSDKコマンド(rcコマンドも含む)に対して，Telloからは応答メッセージが返ってきます．
forward 5000やleft 4000などの様な長距離の移動コマンドを打つと，コマンドを完了してTelloから応答が来るまで長時間待たされることになります．

しかし，ラジコン感覚で操縦することの重要な点は 即時性 です．
なので Telloからの応答を待たず，コマンドを送信するだけ に書き換えました．
たとえWiFiの無線環境が悪くてコマンドの取りこぼしがあったとしても，すぐに次のコマンドが送られて来るので，わざわざ応答なんか待つ必要がないのです．

50ミリ秒のウェイトに変更した点と合わせて，**「データは一方通行＆投げまくり」**になったのです(^_^

#おわりに

Tello SDKに基づいて，送信するコマンドと送信タイミングを変更するだけで，動作が機敏になりました．

今後もこのプログラムを参考に，M5stackのgloveポートを使ったり，ESP32で操作するプログラムを紹介します．