DJI Mavic3を自律飛行させる方法｜プロポ外部操作編２

ロボットアームの導入

離陸＆着陸の動作も自動化したい。
更にMavic 3には人物をカメラで追う機能もある。
ということで諸々のタッチパネルの動作を自律的におこなえるようにタッチ画面を操作するためのロボットアームを導入。

uArm Swift Pro

セグウェイをリモート操作するためのiOSアプリを操作させるために2018年からこのuArm Swift Proというロボットアームを使い続けてきて、Processingで書いたソフトも現役で動くので、今回もこちらを継続して使う。

GcodeのコマンドでXYZ座標を指定してアームを動かしたいポジションに移動させる。
もちろんセグウェイを操作するインターフェイスとは違うので、DJIのプロポ用にソフトウェアを多少書き換える

ちなみに買った当初はジャスト10万円ぐらいだったはずだけど、今調べてみたらかなり値上がりしてた
https://amzn.asia/d/czSyl9y

本家サイトみたら該当商品なかったのでもう生産してないのかもしれない。

プロポ画面の操作

離陸時

• 左端の上向き矢印をタップ
• 真ん中に出てくる丸いボタンを2秒ぐらいタップ

着陸時

• 離陸時と同様の手順
• たまに「着陸ポジションほんとにここでOK？」みたいなアラート出るので最後にOKボタンをタップするようにする

と非常にシンプルで単純な動作。
以下、Processingのプログラム。
MaxMSPからudpsendオブジェクト使ってP5にコマンド送って操作するような仕様。

armController.pde

import processing.serial.*;
import oscP5.*;
import netP5.*;

Serial port;
OscP5 oscP5;
NetAddress remoteLocation;

boolean down = false;
boolean fixPos = false;
boolean randMov = false;

int speed = 30;
float centerX = 152.0;
float centerY = 0.0;
float sideBtX = 137.0;
float sideBtY = 55.0;
float u = 70.0;
float d = 60.0;
float safe = 70.0;
float posX = centerX;
float posY = centerY;
float posZ = safe;

float val = 0.5;

String cmnd;
String lastState = "S";

void setup() {

  size(1, 1);
  oscP5 = new OscP5(this, 12001);
  remoteLocation = new NetAddress("127.0.0.1", 12000);

//シリアルポート名要書き換え
  port = new Serial(this, "/dev/cu.usbmodem1421", 115200);
  port.bufferUntil(10);
  port.clear();
  delay(1000);
  port.write("\n");
  delay(2000);
  port.write("G30\n");
  delay(3000);
  cmnd = "G01X" + (posX) + "Y" + (posY) + "Z" + posZ + "F" + speed + "\n";
  port.write(cmnd);

  OscMessage boot = new OscMessage("/boot");
  boot.add(1);
  oscP5.send(boot, remoteLocation);
}


void draw() {
  background(0);
  noStroke();
  fill(255);
}

void serialEvent(Serial p) {
}

void keyPressed() {
  //このへんは座標を把握するためのコントロールモード
  if (fixPos) {
    if (key == CODED) {
      if (keyCode == UP) {
        if (posX < 370.0) posX += val;
      } else if (keyCode == DOWN) {
        if (90.0 < posX) posX -= val;
      } else if (keyCode == LEFT) {
        if (posY < 70.0) posY += val;
      } else if (keyCode == RIGHT) {
        if (-70.0 < posY) posY -= val;
      }
    }
    cmnd = "G01X" + (posX) + "Y" + (posY) + "Z" + posZ + "F" + speed + "\n";
    port.write(cmnd);
    println(cmnd);
  }
}
void keyReleased() {
  if (!fixPos) {
    if (key == CODED) {
      if (keyCode == UP) {
      }
    }
  }

  if (key == ' ') {
    if (fixPos) {
      down = !down;
      if (down) posZ = d;
      else posZ = u;
      movArm(posX, posY, posZ, 1000);
    }
  } else if (key == BACKSPACE) {
    initMov("T");
  } else if (key == ENTER) {
    fixPos = !fixPos;
    if (!fixPos) {
      posZ = u;
      posX = centerX;
      posY = centerY;
    }
  }
  println(fixPos);
}

void oscEvent(OscMessage theOscMessage) {
  if (theOscMessage.checkAddrPattern("/arm")) {
    int cmndMax = theOscMessage.get(0).intValue();
    //Maxでキータイプしたら一連の動作
    //T = Take Off 離陸
    //L = Landing 着陸
    if (cmndMax == 1) initMov("T");
    else if (cmndMax == 2) initMov("L");
  }
}
void initMov(String state) {
  movArm(sideBtX, sideBtY, u, 1000);
  movArm(sideBtX, sideBtY, d, 50);
  movArm(sideBtX, sideBtY, u, 100);
  movArm(centerX, centerY, u, 1000);
  movArm(centerX, centerY, d, 2000);
  movArm(centerX, centerY, u, 3000);
  //L = 着陸だった場合だけOKボタンのところをタップ
  if (state == "L") {
    posX = 139.0;
    posY = -12.0;
    movArm(posX, posY, u, 100);
    movArm(posX, posY, d, 50);
    movArm(posX, posY, u, 100);
  }
}

void movArm(float pX, float pY, float  pZ, int d) {
  cmnd = "G01X" + (pX) + "Y" + pY + "Z" + pZ + "F" + speed + "\n";
  port.write(cmnd);
  delay(d);
}

画面の中に映ってる人物を指で四角く囲うようにタップ＋スワイプすると人間のシルエットを検知してその人物をカメラが追うようになる。
その部分はまた後日実装したい。

一通りの基本操作外部化＆位置＋姿勢把握 完了

動作確認デモ

課題＋今後

屋内使用は基本想定してないせいなのか？実際にMavic3飛ばす空間でも飛行テストして起きたことだけど、無地の単色の壁を壁として認識できず、機体の物体認識反応せず壁に激突してしまう（飛行テストで一度壁にぶつかって墜落）。
そこで外付したシングルポイントLiDAR TSD10の測定値で激突回避できると思いきや、ドローンの移動速度がけっこう速く、動き止めても慣性で移動方向にけっこう動いてしまう。
データがPCに届くまでのレイテンシーも関係してくる。
とにかく壁に激突回避すべく何か工夫が必要。

その問題解決しつつ、実際の空間想定して動きのシーケンスをMaxで組んでいく。
そのために実際の空間に近しい広い空間での開発が必要。。