TELLOドローンをC++Builderでプログラムして飛ばしてみた

### 概要
[TELLO](https://www.ryzerobotics.com/jp/tello)より販売されているドローンは、初心者向けで、免許も必要なく、比較的に安価に入手できる。カメラも付いており手軽に空撮が楽しめる。更にTELLOのドローンの特徴として、UDPでコマンドを送って、操縦する事ができる。PythonでのTELLOの操作は良く目にするが、今回は、私が普段使いのEmbarcadero C++Builderを使って、C++でTelloをプログラミングで飛ばしてみたい。
### C++Builder
[C++Builder](https://www.embarcadero.com/jp/products/cbuilder)は、Bolandの時代から使っている。最近のC++BuilderはWindowsだけでなくMac,Android,iOSにも対応したクロスコンパイル機能までついている。今回は、最新のC++Builder10.3 Community Editionを使った。Community Editionは個人で趣味の範囲で利用する分には無償で利用できる。
### TELLOドローン
TELLOのドローンは、UDPでコマンドを送る事で操縦が行える。公開されている仕様は[ここ](https://www.ryzerobotics.com/jp/tello/downloads)のTELLO SDKより参照できる。UDPの8889ポートよりコマンドを送る事で制御できる。

| コマンド       |    説明 | 
|:--------------|:------------------|
| command       | SDKモードに切替 |
| takeoff       | 離陸 |  
| land          | 着陸 | 
| streamon      | カメラストリームON |
| streamoff     | カメラストリームOFF | 
| up x          | x cm上昇 |
| down x        | x cm下降 |
| left x        | x cm左移動 |
| right x       | x cm右移動 |
| forward x     | x cm前移動 |
| back x        | x cm後移動 |
| cw x          | x 度時計方向に回転 |
| ccw x         | x 度反時計方向に回転 |

###サンプルコード
実際に作成したサンプルのコードを示す。
画面には、StartのボタンとUDP通信の為のIndyのUDPClient1と、姿勢位置情報受信用のIdUDPServer1を配置した。位置情報を表示するためのStatusBarも付けた。
![scrn.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/606353/daf91611-a4d9-7541-f91b-24c859dbaa86.jpeg)

以下がサンプルコードになる。

```C++:Unit1.cpp
//---------------------------------------------------------------------------

#include <vcl.h>
#include <jpeg.hpp>

#pragma hdrstop

#include "Unit1.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma resource "*.dfm"
TForm1 *Form1;
//---------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner)
	: TForm(Owner)
{
}
//---------------------------------------------------------------------------
void __fastcall TForm1::Button1Click(TObject *Sender)
{	// push button 
	IdUDPClient1->Port = 8889;
	IdUDPClient1->Host = "192.168.10.1";
	IdUDPClient1->Active = True;

	ccmd();

	if(tcmd("command")<0) return;
	if(tcmd("streamon")<0) return;
	if(tcmd("takeoff")<0) return;
	if(tcmd("back 20")<0) return;
	if(tcmd("land")<0) return;
}
//---------------------------------------------------------------------------
void __fastcall TForm1::FormShow(TObject *Sender)
{	// Form Show
	IdUDPServer1->DefaultPort = 8890;
	IdUDPServer1->Active = True;
}
//---------------------------------------------------------------------------
void __fastcall TForm1::IdUDPServer1UDPRead(TIdUDPListenerThread *AThread, const TIdBytes AData,
			 TIdSocketHandle *ABinding)
{	//
	UnicodeString req = BytesToString(AData);
	StatusBar1->SimpleText = req;
}
//---------------------------------------------------------------------------
int TForm1::tcmd(String com)
{	// excute tello command
	int   i;
	String res;

	res = "";
	while (!(IdUDPClient1->ReceiveString(50).IsEmpty()))
				;

	IdUDPClient1->Send(com);
	for(i=1;i<10;i++){
		Sleep(1000);
		res = IdUDPClient1->ReceiveString(50);
		if(res!="") break;
	}
	if(res=="ok") return 1;
	else return -1;
}
//----------------------------------------------------------------------------
int TForm1::ccmd(void)
{	// start ffmpeg camera
	String 				  cmd;
	wchar_t             bt[1024];
	TStartupInfo        SI;
	TProcessInformation PI;

	cmd = ExtractFilePath(Application->ExeName) + "ffplay.exe -probesize 32 -sync ext udp://127.0.0.1:11111";

	wcscpy_s(bt,254,cmd.c_str());
	ZeroMemory(&SI,sizeof(SI));
	ZeroMemory(&PI,sizeof(PI));

	SI.cb = sizeof(SI);
	GetStartupInfo((LPSTARTUPINFO)&SI);

	SI.dwFlags |= STARTF_USESHOWWINDOW;
	SI.wShowWindow = SW_SHOW;

	CreateProcess(0,bt,0,0,false,CREATE_DEFAULT_ERROR_MODE,0,0,(LPSTARTUPINFO)&SI,&PI);
}
//---------------------------------------------------------------------------
```

起動時に姿勢位置情報受信用のIdUDPServer1にポート番号を設定して、Activeにしている。Startボタンを押すと、IdUDPClient1を使って、TELLOドローンのIP(192.168.10.1:固定)にUDPで接続して、command(SDKモードに切替), streamon(カメラストリームON),takeoff(離陸), back 20(後退), land(着陸)のコマンドを送る。更に、UDPServer1に位置情報を受信するとStatusBarに表示するようにしている。Telloへのコマンド実行はtcmd関数で行っており、UDPでコマンド送信後、通信の返信を待っている。TELLOのカメラの映像は、[ffplay.exe](https://www.ffmpeg.org/)を使うことにしている。実行ファイルと同じフォルダに、ダウンロードしたffplay.exeを置いて、ccmd()の関数でffplay.exeを起動している。
###結果
WindowsのPCで実行する。TELLOドローンにPCの無線LANを接続する。ソフトをコンパイルし、実行すると希望通りに離陸から着陸まで、ドローンを操縦する事ができる。ステータスバーには、ドローンの位置情報が表示され更新される。フォーマットは次のようになる。
“pitch:%d;roll:%d;yaw:%d;vgx:%d;vgy%d;vgz:%d;templ:%d;temph:%d;tof:%d;h:%d;bat:%d;baro:%.2f; time:%d;agx:%.2f;agy:%.2f;agz:%.2f;\r\n”
![DSC_1580a.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/606353/99929012-b913-82f9-0bd9-5c49b5da5f0d.jpeg)
###課題
今回、ドローンをC++Builderを作成したソフトウェアから操縦した。次の点を課題として残している。
- カメラストリームの処理をffplay.exeで行ったが、カメラの画像や動画を保存したりしたい。

これらの課題については、解決次第、本記事を更新する。
###追記
2020/5/10　　カメラ映像をffplay.exeを使って実装した事で記事を更新。