1. 소개

드론 「Tello」를 「M5StickC」로 컨트롤하는 스케치를 만들어 보았습니다.

2. 환경

"Tello"와 "M5StickC"사이는 WiFi 무선, UDP 프로토콜로 통신합니다.
Tello 명령은 공식 Tello SDK를 사용했습니다.
htps //w w. ry 제로 보치 cs. 코 m / jp / 헉 / 드 w ぉ 아 ds

2-1. 개발 환경

· Arduino IDE 1.8.9

3. 조작 방법

(1) 버튼 조작
　　・버튼A(중앙)-----시계 방향으로 45도 회전
　　·버튼 B(우측면)-----이륙과 착륙 토글 전환

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

(3)상·하 이동
　　・후퇴에 기울이면서, 버튼A(가운데)를 누른다-----상승
　　・전진에 기울이면서, 버튼A(가운데)를 누른다-----하강

4. 스케치

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;
}

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에서 받은 메시지

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 착륙)

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 기준값의 조정
　　142행
　　　　if (fabs(y)> 0.5){
　　161행
　　　 　if (fabs(x)> 0.5){{
　　173행
　　　 　if (fabs(y)> 0.5){
좌우 전후의 이동이 잘 되지 않을 때는 이 값을 조정해 주십시오.

6. 끝에

가속도 X, Y의 보정치를 취득하기 위해서, 10회 더미로 가속도를 판독한 후에, 10회 판독을 행하고 나서 그 평균치를 보정치로 했습니다.
위치 정보는 4원수를 사용하면 보다 정확해진다고 생각합니다만, 이번은 간이적인 방법으로 실시했습니다.