C#とTELLOでいろいろやってみる!

なんか、SDKが更新されているような気がしたため、久しぶりにドローン(TELLO)を制御するプログラムを書きました。
(もしかしたらきのせいかのしれません。。。)
ちなみに、今回参照したSDKのバージョンは、1.3.0.0です。

今回は、TELLOのバッテリー残量の取得など、公開されているSDKコマンドを使用してTELLOのデータの取得を行ってみました。
また、UDPを使ったコマンドの送受信を行う処理の共通化も行ってみました。
(送信と受信は異なる内容のため、メソッドを分けてます)

↓↓↓ちなみにTELLOは、こちらのドローンです↓↓↓

Tello トイドローン（損害賠償保険付）+ 予備バッテリー 1本

出版社/メーカー: DJI
メディア: エレクトロニクス
この商品を含むブログを見る

実際に書いたC#のプログラム

まずは、UDPによるコマンドの送信についてです。
UDPによるコマンドの送信では、送信するコマンドが異なることを除いて、同じ処理を行います。

そのため、引数にコマンド文字列を持つメソッド"SendCommand"を作成しています。
実際のプログラムは以下の通りです。

private UdpClient udpClient = null;

// TELLO接続文字列
private const string CONNECT_TELLO_IP = "192.168.10.1";
private const int CONNECT_TELLO_PORT = 8889;

/// <summary>
/// コマンドの送信
/// </summary>
/// <param name="commandTxt"></param>
/// <returns></returns>
private int SendCommand(string commandTxt)
{
	// UDPクライアントを生成する
	if(udpClient == null)
	{
		udpClient = new UdpClient();
	}

	// TELLOへ送信するコマンドを生成する
	string connectMessage = commandTxt;
	byte[] byteMessage = Encoding.UTF8.GetBytes(connectMessage);

	// TELLOへコマンドを送信する
	int rcv = udpClient.Send(byteMessage, byteMessage.Length, CONNECT_TELLO_IP, CONNECT_TELLO_PORT);

	return rcv;
}

UDPでTELLOにコマンドを送信するために、UdpClientクラスを使用します。(UdpClientはSystem.Net.Socketsにいます)
今回は、UdpClientクラスをメンバー変数として持たせることで、2回目以降"SendCommand"メソッドが呼ばれた際にnewをしなくていいようにしています。
(newは重たい処理だと聞いたことがあるため、あまり何度もnewをしたくなかった)

そして、UdpClientの"Send"メソッドを使用して、コマンドの送信を行います。
コマンドを送信する際は、送信コマンドをバイト配列に変換する必要があります。
また、"Send"メソッドでコマンド送信する際は、IPアドレスとポート番号を一緒に引数に指定します。

次にUDPによるTELLOからのデータの受信処理です。

/// <summary>
/// コマンドの受信
/// </summary>
/// <returns></returns>
private bool UdpReciever(int recievePort,string recieveIP,int recieveMode)
{
	var canGetRecieveMessage = false;

	// TimeOutを設定する
	udpClient.Client.ReceiveTimeout = 300;
	IPAddress telloAddress = null;

	// IPとポートをバインドする
	telloAddress = IPAddress.Parse(recieveIP);

	// UDPのEndPointを生成する
	var endPoint = new IPEndPoint(telloAddress, recievePort);

	byte[] rcvByte = null;

	try
	{
		// UDPを使用して、TELLOからデータを受信する
		rcvByte = udpClient.Receive(ref endPoint);
	}
	catch (Exception ex)
	{
		return canGetRecieveMessage;
	}

	if (rcvByte != null && rcvByte.Length > 0)
	{
		// 受信データを文字列に変換する
		var recieveMessage = Encoding.UTF8.GetString(rcvByte);

		if(recieveMode == (int)RecieveMode.OKCancel)
		{
			if (recieveMessage == "ok")
			{
				canGetRecieveMessage = true;
			}
		}
		else if(recieveMode == (int)RecieveMode.GetData)
		{
			this.textBox1.Text = recieveMessage;
			canGetRecieveMessage = true;
		}
	}

	return canGetRecieveMessage;
}

データ受信では、UdpClientの"Receive"メソッドを使用して、コマンドの受信を行っています。
"Receive"メソッドでは、IPEndPointを引数に渡して、データを受信します。

そして、バッテリー残量の確認です。

/// <summary>
/// バッテリー残量の確認
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button5_Click(object sender, EventArgs e)
{
	SendCommand("battery?");
	UdpReciever(CONNECT_TELLO_PORT, CONNECT_TELLO_IP,(int)RecieveMode.GetData);
}

バッテリー残量の確認は、ボタンを押下することで、画面のテキストボックスにバッテリー残量が表示されるようにしております。

また、コマンドの送受信は、上記で作成したメソッドを使用しています。
そのため、コマンドの送信は、バッテリー残量を確認するためのコマンドである"battery?"を引数として、"SendCommand"メソッドを実行するだけとなっております。
受信についても同様に"UdpReciever"メソッドの実行のみとなっています。