AR.Drone for AS3 で Drone から送信されてくる情報を受信して解析する

公式の SDK は一切使わず（というか使えず…）コードは全てフルスクラッチで書いているためカメのようにのろい開発速度ながらも、やってるうちになんとなく機能が増えていっている AR.Drone for AS3 ですが…

今回は Drone から送信されてくる情報を受信して解析し、Drone のバッテリーの残量や前後左右の傾きと回転方向（Roll, Pitch, Yaw）や高度、移動距離などが取得できるようになりました。

で、せっかくこういった情報が取得できるようになったので、Flash でテキトーに作った UI に反映するように修正して飛ばしてみました。
　

　
この映像では、まずバッテリーの残量を UI 左上のゲージに表示し、傾きを中央左側の Drone の写真に反映させて、一応３次元で表現しています。（あまり大きく傾かないので分かりづらいですが…）
また、高度を中央右側の高度計に反映させるようにしてみました。

この飛行データの取得方法ですが、今回は下記のような流れで実装しました。

1. AR.Drone から送信されてくる NavigationData を NavigationData 用ポートで接続した UDPソケットで受信。

private function rcvNaviData( e:DatagramSocketDataEvent ):void
{
	_parser.parseNavData( e.data );
}

2. 受信したバイナリデータを AR.Drone SDK Developer Guide を四苦八苦して読みながらパース。
（このときパースする受信データのバイト順序はリトルエンディアンで）

public function parseNavData( buffer:ByteArray ):void
{
	buffer.endian = Endian.LITTLE_ENDIAN;
	var magic:int = buffer.readInt();
	var state:int = buffer.readInt();
	var sequence:Number = buffer.readInt() & 0xFFFFFFFF;
	var vision:int = buffer.readInt();
		
	if( sequence <= _lastSequenceNumber && sequence != 1 )
	{
		return;
	}
	_lastSequenceNumber = sequence;
	
	if( buffer.position < buffer.length )
	{	
		var tag:int = buffer.readShort() & 0xFFFF;
		var payloadSize:int = (buffer.readShort() & 0xFFFF)-4;
		
		var controlState:int = buffer.readInt();
		
		batteryPercentage = buffer.readInt();
		pitch = buffer.readFloat()/1000;
		roll = buffer.readFloat()/1000;
		yaw = buffer.readFloat()/1000;
		altitude = buffer.readInt();
		vx = buffer.readFloat();
		vy = buffer.readFloat();
		vz = buffer.readFloat();
				
		dispatchEvent( new Event( "state_change" ) );
	}
}

3. パースしたデータを UI に反映。

と、まぁ、このような感じでやってみました。

というわけで…

このままでは飛行安定性に不安は多々残るものの、これで一応「操作」と「航行データの取得」ができるようになったので、次は Drone に搭載されている２台のカメラからの映像取得に取り組みたいと思っています。

っていうかこれ、Flash から Drone を操作する API として公開できる日が来るのだろうか…