C#でドローンプログラミング-高さが30cm～130cmの間でUP-DOWNさせてみた

C#を使って、ドローン(TELLO)の飛行制御をしました。

↓↓↓使用したドローン(TELLO)は、こちらの機体です↓↓↓

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

今回は、ドローンの飛行している高度を取得するコマンドと、ドローンを上昇させるコマンド及び、下降させるコマンドを使用して、一定の高さの範囲を上下に移動するプログラムを組みました。
今回のプログラムでは、Startボタンをクリックすることで、TELLOが上下に移動をはじめ、Stopボタンをクリックすることで、TELLOが上下に移動することをやめるようにしております。

実際のプログラムでは、Startボタンがクリックされたタイミングで、別タスクを開始させ、TELLOの上下移動の制御は、別タスクの中で非同期に実行するようにしています。
また、タスク内では、Stopフラグがtrueになるまで上下移動を続けるように処理を記載しています。
そして、Stopボタンがクリックされたタイミングで、Stopフラグをtrueに変更することで、別タスクで行っているTELLOの上下移動のループ処理を終了するようにしております。

1.Startボタンの処理

Task upDownTask;

/// <summary>
/// テローを一定の高さの範囲でUp-Downさせる
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void StartUpDown_Click(object sender, EventArgs e)
{
	isStopUpDown = false;

	this.StartUpDown.Enabled = false;

	// タスクの実行
	upDownTask = new Task(DoUpDown);
	upDownTask.Start();

}

ドローンを上下させる処理は、DoUpDownメソッドを作成し、DoUpDownメソッドへ実装しています。
そして、ドローンを上下させるDoUpDownメソッドを引数として、Taskを起動しています。

2.ドローンを上下させる処理

/// <summary>
/// TELLOをUP/DOWNをさせる処理
/// </summary>
private void DoUpDown()
{
	var returnCommand = string.Empty;

	while (!isStopUpDown)
	{
		// 現在の高度を取得する
		this.SendCommand("height?");

		while(true)
		{
			returnCommand = this.UdpReciever(CONNECT_TELLO_PORT, CONNECT_TELLO_IP, (int)RecieveMode.None);

			if(returnCommand.Contains("dm"))
			{
				break;
			}
		}

		var height = CalcTelloHeight(returnCommand);

		if(height == -1)
		{
			break;
		}

		Thread.Sleep(1000);

		if (height >= 130)
		{
			// 高さが130cm以上の場合はdownコマンドで高度を下げる
			this.SendCommand("down 20");
			canUp = false;
		}
		else if(height <= 30)
		{
			// 高さが30cm以下の場合はupコマンドで高度を上げる
			this.SendCommand("up 20");
			canUp = true;
		}
		else
		{
			// 高さが30-130cmの間の場合は、前回と同じコマンドを実行する
			if(canUp)
			{
				var up = 20;

				if(height + 20 > 130)
				{
					up = 130 - height;
				}

				this.SendCommand("up " + up.ToString());
			}
			else
			{
				var down = 20;

				if (height - 20 < 30)
				{
					down = height - 30;
				}

				this.SendCommand("down " + down.ToString());
			}
		}

		returnCommand = this.UdpReciever(CONNECT_TELLO_PORT, CONNECT_TELLO_IP, (int)RecieveMode.None);
		Thread.Sleep(1000);
	}
}

whileループで高度の取得→UPコマンドまたはDOWNコマンドの送信を繰り返し行うようにしています。
whileループの条件は、メンバー変数"isStopUpDown"を使用しています。
そして、メンバー変数"isStopUpDown"の値をStopボタンのクリック処理で値を変更することで、ループを抜けるように制御しています。

3.高度の計算

/// <summary>
/// TELLOの現在の高さを計算する
/// </summary>
/// <param name="recieveMessage"></param>
/// <returns></returns>
private int CalcTelloHeight(string recieveMessage)
{
	var height = 0;

	// 高度は0dm\r\nの形式で受信されるため、dm\r\nの部分を除去する
	// また、dm = 10cmのため、取得した高度を10倍して、cm単位で処理を行う
	var heightString = recieveMessage.Replace("dm\r\n", "");

	if (!int.TryParse(heightString, out height))
	{
		// 高さが取れなかった場合は異常値を返す
		return -1;
	}

	return height * 10;
}

"height?"コマンドで取得できるTELLOの高度は、dm単位となっています。
そして、1dm = 10cmの関係となっております。
また、"height?"コマンドで取得した値は”#0dm\r\n”形式となっております。
そのため、取得した文字列から不要な"dm\r\n"を除去し、数値部分のみを取得しています。

なお、今回は、すべてcm単位で処理を行うため、取得した値を10倍してcm単位の値を戻り値としています。

4.Stopボタンの処理

/// <summary>
/// Up-Downのストップ
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void StopUpDown_Click(object sender, EventArgs e)
{
	isStopUpDown = true;

	upDownTask.Wait();
	upDownTask.Dispose();

	this.StartUpDown.Enabled = true;
}

↓↓体験セットというのもあるらしいです↓↓