Go to Qiita Advent Calendar 2024 Top
LoginSignup
12
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@hsgucci

M5 ATOM MatrixでTelloをコントロールする

Last updated at Posted at 2020-07-18

#参考にしたページ
Ryze Tech. の Tello を M5 ATOMで動かす方法のベースになっているのは，以下のページです．

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

M5Atom Matrixに搭載されているIMUはMPU6886なので，M5StickCと同じです．
当該ページのプログラムを応用することで，
M5Atom MatrixでもTelloを様々に動かすことが可能になります．
ただし，M5Atom LiteはIMUが無いので使えません(>_<

#はじめに
M5StickCの加速度センサーを使ってTelloを操縦する方法は，上記のサイトを利用すれば可能です．

これまで，Tello SDKのrcコマンドを利用して，以下の記事を書きました．

本ページでは，新たに発売された
　　M5Atom Matrixを使ってTelloを操縦
できるようにします．

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

M5AtomはStackやStickCと違って，バッテリーを内蔵していないので，モバイルバッテリーから電力を供給しています(^^

#概要
M5StickCと異なり，M5Atom MatrixやLiteはI/Oとして使えるボタンが１つしかありません．
なので，これに離陸/着陸に割り当ててしまうと，
これまで のように余ったボタンで左右旋回や上昇/下降モードに切り替える事ができなくなってしまいます．．．

そこで，実はM5StackやM5StickCでこっそり実験していた「裏返しモード」をM5Atomに実装する事にしました．
「裏返しモード」とは，これです．

マイコンを裏返すことで，Z軸方向の加速度センサーには負の値が入ります．
if文でZ軸加速度の正負を見て，送信するrcコマンドを切り替えれば良いのです．

システム構成図としては，こんな風になります．
m5atom_tello.png

あ，M5StackやStickCでの裏返しモードの記事も，今後書く予定です(^^;;

#スケッチ
M5Atom Matrixに書き込むスケッチです．

ここ　を右クリックして[名前を付けて保存]機能でファイル保存してください．
日本語バージョンは以下に書いてあります．

TelloControl_Atom.ino
#include <M5Atom.h>
#include <WiFi.h>
#include <WiFiUdp.h>

// TELLOの設定
const char* TELLO_SSID = "TELLO-xxxxxx";  // 自分のTelloのSSIDをここに書く
const char* TELLO_IP = "192.168.10.1";    // TELLOのIPアドレス
const int PORT = 8889;                    // TELLOのPORT

// WiFi関連の変数
WiFiUDP Udp;
char packetBuffer[255];
String message = "";
char command_str[20];

// MPU6886関連の変数
bool IMU6886Flag = false;

// 加速度を格納する変数
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float x, y, z;

// setup関数はリセット時１度だけ実行される:
void setup() {
  M5.begin();
  // ATOMのMPU6886のサンプルと同じ処理
  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

  // 初期化
  Wire.begin();
  WiFi.begin(TELLO_SSID, "");   // default Tello has no password

  // WiFiでTelloに接続するまで待ち続ける
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  // UDPクライアント開始
  Udp.begin(PORT);

  // "command"をTelloに送信すると，SDKモードに変わる
  tello_command_exec("command");

  delay(500); // 安定のための待ち時間
}

// loop関数は繰り返し呼び出される．永久ループ:
void loop() {
  // センサーから加速度を取得する
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  x = accX;
  y = accY;
  z = accZ;

  // ボタンBの処理
  // 着陸
  if (M5.Btn.wasPressed()) {  // ボタンBを押してすぐ離すと，着陸
    tello_command_exec("land");
    delay(500);
  }
  // 離陸
  if (M5.Btn.pressedFor(300)) { // ボタンBを300msec押し続けると，離陸
    tello_command_exec("takeoff");
    delay(500);
  }

  // ATOM Matrixは，LEDが上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

  M5.update();
}

// UDP send command
void tello_command_exec(char* tello_command) {
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(tello_command);
  Udp.endPacket();
  delay(100);
}

#スケッチ解説

スケッチは，
　M5StickCを使う元祖の記事
と
　M5ATOMのMPU6886を利用するサンプル
を融合させる形で作成しました．

ATOMは，ArduinoのボードとしてはM5Stick-Cをセットします．
マイコン自体はM5StickCと同じなので．

##include
includeで気をつけたいのは，ATOMはM5Atom.hを使う点です．
M5StickCのときはM5StickC.hでした．変更し忘れないよう注意しましょう．

include
#include <M5Atom.h>
#include <WiFi.h>
#include <WiFiUdp.h>

##グローバル変数
TelloのSSIDの記述や，WiFiに関する部分は，M5StickCを使う元祖の記事のままです．

グローバル変数
// TELLOの設定
const char* TELLO_SSID = "TELLO-xxxxxx";  // 自分のTelloのSSIDをここに書く
const char* TELLO_IP = "192.168.10.1";    // TELLOのIPアドレス
const int PORT = 8889;                    // TELLOのPORT

// WiFi関連の変数
WiFiUDP Udp;
char packetBuffer[255];
String message = "";
char command_str[20];

// MPU6886関連の変数
bool IMU6886Flag = false;

// 加速度を格納する変数
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float x, y, z;

元祖の記事では，電源投入後に加速度センサの値を何回か取得してオフセット量を設定する処理がありました．
しかし，小さなATOMはUSBケーブルのテンションでも傾いてしまうので，水平ではない姿勢でリセットした場合には，逆に変なバイアスがかかってしまい，水平にしてるのにTelloが動く，という現象が多発しました．
なのでオフセット計算の部分は，ざっくりオミットしました．

また，裏返しモードを使うために，Z軸の加速度を取るfloat型変数も用意しました．

##setup関数
setup関数は，ほぼ元祖の記事と同じですが，StickCと異なる部分を削除しました．

  • ATOMは液晶画面が無いので，LCD関連の関数を削除
  • 加速度のキャリブレーション(オフセット値計算）を削除

おかげで，ずいぶんスッキリしました．

setup関数
// setup関数はリセット時１度だけ実行される:
void setup() {
  M5.begin();
  // ATOMのMPU6886のサンプルと同じ処理
  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

  // 初期化
  Wire.begin();
  WiFi.begin(TELLO_SSID, "");   // default Tello has no password

  // WiFiでTelloに接続するまで待ち続ける
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  // UDPクライアント開始
  Udp.begin(PORT);

  // "command"をTelloに送信すると，SDKモードに変わる
  tello_command_exec("command");

  delay(500); // 安定のための待ち時間
}

##loop関数
loop関数も，LCD関連の命令がなくなったので，読みやすくなっています．

loop関数
void loop() {
  // センサーから加速度を取得する
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  x = accX;
  y = accY;
  z = accZ;

  // ボタンBの処理
  // 着陸
  if (M5.Btn.wasPressed()) {  // ボタンBを押してすぐ離すと，着陸
    tello_command_exec("land");
    delay(500);
  }
  // 離陸
  if (M5.Btn.pressedFor(300)) { // ボタンBを300msec押し続けると，離陸
    tello_command_exec("takeoff");
    delay(500);
  }

  // ATOM Matrixは，NeoPixelが上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

  M5.update();
}

ポイントは，ボタンBの処理に使われている２つの命令です．

  • M5.Btn.pressed()：ボタンを押してすぐ離した場合
  • M5.Btn.pressedFor(300)：ボタンを引数で与えたミリ秒間押し続けた場合

離陸する際には，ボタンBを長押しします．
一方，ボタンをポンッと短押しすれば着陸です．
予想外の挙動をしたり，危ないときなどにはパニックになるので，長押しで着陸にはしませんでした．

##裏返しモード
ATOMは，Z加速度の値が，我々の一般的感覚と逆です．
m5atom_accZ.png
おそらく回路基板に上下逆に搭載されているんでしょう．
っと思っていたら，ATOMを分解した猛者の方がいました．
M5Atom(M5Atom MatrixとM5Atom Lite)を手に入れたので調べてみた
この写真を見ると，
m5atom_open.png
基板の上面にNeoPixelが，裏側にIMUがあるのがわかります．

と，言う訳で，プログラムの際には正・負を逆にして書かなければなりません．

ATOMを裏返したときに処理を変える
  // ATOM Matrixは，NeoPixel側が上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

それ以外の処理は，これまでのrcコマンドと同じです．
意外と簡単にかけることがわかったかと思います．

#おわりに

M5Atomの様な超小型のマイコンでTelloが動かせるとなると，手袋に仕込んだり，魔法の杖や剣っぽい物に仕込んだりして，格好良く作りたくなってきます．

　
　
　

12
3
2

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
12
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?