LoginSignup
15
7

More than 3 years have passed since last update.

@Mitsu-Murakita(Mitsuaki Murakita)

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

Posted at

1. はじめに

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

２. 環境

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

２－１. 開発環境

・Arduino IDE 1.8.9

３. 操作方法

image.png

（１）ボタン操作
　　　・ボタン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回読み取りを行なってからその平均値を補正値としました。
位置情報は四元数を使えばより正確になると思いますが、今回は簡易的な方法で行いました。

15
7
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
15
7