upload source files

ARDroneForP5_Sample.pde

+import com.shigeodayo.ardrone.processing.*;
+
+ARDroneForP5 ardrone;
+
+void setup() {
+  size(320, 240);
+
+  ardrone=new ARDroneForP5("192.168.1.1");
+  // connect to the AR.Drone
+  ardrone.connect();
+  // for getting sensor information
+  ardrone.connectNav();
+  // for getting video informationp
+  ardrone.connectVideo();
+  // start to control AR.Drone and get sensor and video data of it
+  ardrone.start();
+}
+
+void draw() {
+  background(204);  
+
+  // getting image from AR.Drone
+  // true: resizeing image automatically
+  // false: not resizing
+  PImage img = ardrone.getVideoImage(false);
+  if (img == null)
+    return;
+  image(img, 0, 0);
+
+  // print out AR.Drone information
+  ardrone.printARDroneInfo();
+
+  // getting sensor information of AR.Drone
+  float pitch = ardrone.getPitch();
+  float roll = ardrone.getRoll();
+  float yaw = ardrone.getYaw();
+  float altitude = ardrone.getAltitude();
+  float[] velocity = ardrone.getVelocity();
+  int battery = ardrone.getBatteryPercentage();
+
+  String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude;
+  text(attitude, 20, 85);
+  String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1];
+  text(vel, 20, 140);
+  String bat = "battery:" + battery + " %";
+  text(bat, 20, 170);
+}
+
+//PCのキーに応じてAR.Droneを操作できる．
+// controlling AR.Drone through key input
+void keyPressed() {
+  if (key == CODED) {
+    if (keyCode == UP) {
+      ardrone.forward(); // go forward
+    } 
+    else if (keyCode == DOWN) {
+      ardrone.backward(); // go backward
+    } 
+    else if (keyCode == LEFT) {
+      ardrone.goLeft(); // go left
+    } 
+    else if (keyCode == RIGHT) {
+      ardrone.goRight(); // go right
+    } 
+    else if (keyCode == SHIFT) {
+      ardrone.takeOff(); // take off, AR.Drone cannot move while landing
+    } 
+    else if (keyCode == CONTROL) {
+      ardrone.landing();
+      // landing
+    }
+  } 
+  else {
+    if (key == 's') {
+      ardrone.stop(); // hovering
+    } 
+    else if (key == 'r') {
+      ardrone.spinRight(); // spin right
+    } 
+    else if (key == 'l') {
+      ardrone.spinLeft(); // spin left
+    } 
+    else if (key == 'u') {
+      ardrone.up(); // go up
+    }
+    else if (key == 'd') {
+      ardrone.down(); // go down
+    }
+    else if (key == '1') {
+      ardrone.setHorizontalCamera(); // set front camera
+    }
+    else if (key == '2') {
+      ardrone.setHorizontalCameraWithVertical(); // set front camera with second camera (upper left)
+    }
+    else if (key == '3') {
+      ardrone.setVerticalCamera(); // set second camera
+    }
+    else if (key == '4') {
+      ardrone.setVerticalCameraWithHorizontal(); //set second camera with front camera (upper left)
+    }
+    else if (key == '5') {
+      ardrone.toggleCamera(); // set next camera setting
+    }
+  }
+}
+

src/com/shigeodayo/ardrone/ARDrone.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+
+package com.shigeodayo.ardrone;
+
+import java.awt.image.BufferedImage;
+import java.net.InetAddress;
+import java.net.UnknownHostException;
+import java.util.StringTokenizer;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.command.CommandManager1;
+import com.shigeodayo.ardrone.command.CommandManager2;
+import com.shigeodayo.ardrone.navdata.AttitudeListener;
+import com.shigeodayo.ardrone.navdata.BatteryListener;
+import com.shigeodayo.ardrone.navdata.DroneState;
+import com.shigeodayo.ardrone.navdata.NavDataManager;
+import com.shigeodayo.ardrone.navdata.NavDataManager1;
+import com.shigeodayo.ardrone.navdata.NavDataManager2;
+import com.shigeodayo.ardrone.navdata.StateListener;
+import com.shigeodayo.ardrone.navdata.VelocityListener;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+import com.shigeodayo.ardrone.utils.ARDroneInfo;
+import com.shigeodayo.ardrone.utils.ARDroneVersion;
+import com.shigeodayo.ardrone.video.ImageListener;
+import com.shigeodayo.ardrone.video.VideoManager;
+import com.shigeodayo.ardrone.video.VideoManager1;
+import com.shigeodayo.ardrone.video.VideoManager2;
+
+public class ARDrone implements ARDroneInterface {
+
+	private String ipaddr = null;
+	private InetAddress inetaddr = null;
+
+	// managers
+	private CommandManager manager = null;
+	private VideoManager videoManager = null;
+	private NavDataManager navdataManager = null;
+
+	// listeners
+	private ImageListener imageListener = null;
+	private AttitudeListener attitudeListener = null;
+	private BatteryListener batteryListener = null;
+	private StateListener stateListener = null;
+	private VelocityListener velocityListener = null;
+
+	private ARDroneVersion ardroneVersion = null;
+
+	/** constructor */
+	public ARDrone() {
+		this(ARDroneConstants.IP_ADDRESS, null);
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ipaddr
+	 */
+	public ARDrone(String ipaddr) {
+		this(ipaddr, null);
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ardroneVersion
+	 */
+	public ARDrone(ARDroneVersion ardroneVersion) {
+		this(ARDroneConstants.IP_ADDRESS, ardroneVersion);
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ipaddr
+	 * @param ardroneVersion
+	 */
+	public ARDrone(String ipaddr, ARDroneVersion ardroneVersion) {
+		this.ipaddr = ipaddr;
+		this.ardroneVersion = ardroneVersion;
+		/*System.out.println("hoge");
+		if (ardroneVersion == null)
+			ardroneVersion = new ARDroneInfo().getDroneVersion();
+		System.out.println("AR.Drone version:" + ardroneVersion);*/
+	}
+
+
+	
+	/** connect to AR.Drone */
+	@Override
+	public boolean connect() {
+		return connect(false);
+	}
+
+	private boolean connect(boolean useHighRezVideoStreaming) {
+		if (inetaddr == null) {
+			inetaddr = getInetAddress(ipaddr);
+		}
+		if (ardroneVersion == null)
+			ardroneVersion = new ARDroneInfo().getDroneVersion();			
+		
+		//System.out.println("(connect) AR.Drone version:" + ardroneVersion);
+
+		if (ardroneVersion == ARDroneVersion.ARDRONE1)
+			manager = new CommandManager1(inetaddr, useHighRezVideoStreaming);
+		else if (ardroneVersion == ARDroneVersion.ARDRONE2)
+			manager = new CommandManager2(inetaddr, useHighRezVideoStreaming);
+		else {
+			error("Cannot create Control manager", this);
+			error("Maybe this is not AR.Drone?", this);
+			return false;
+		}
+
+		return manager.connect(ARDroneConstants.PORT);
+	}
+
+	/** connect video */
+	@Override
+	public boolean connectVideo() {
+		if (inetaddr == null) {
+			inetaddr = getInetAddress(ipaddr);
+		}
+		//System.out.println("(connect video) AR.Drone version:" + ardroneVersion);
+
+		if (ardroneVersion == ARDroneVersion.ARDRONE1) {
+			videoManager = new VideoManager1(inetaddr, manager);
+		} else if (ardroneVersion == ARDroneVersion.ARDRONE2) {
+			videoManager = new VideoManager2(inetaddr, manager);
+		} else {
+			error("Cannot create Video manager", this);
+			error("Maybe this is not AR.Drone?", this);
+			return false;
+		}
+
+		videoManager.setImageListener(new ImageListener() {
+			@Override
+			public void imageUpdated(BufferedImage image) {
+				if (imageListener != null) {
+					imageListener.imageUpdated(image);
+				}
+			}
+		});
+		return videoManager.connect(ARDroneConstants.VIDEO_PORT);
+	}
+
+	/** connect navdata */
+	@Override
+	public boolean connectNav() {
+		if (inetaddr == null) {
+			inetaddr = getInetAddress(ipaddr);
+		}
+		
+		//System.out.println("(connect nav) AR.Drone version:" + ardroneVersion);
+
+		if (ardroneVersion == ARDroneVersion.ARDRONE1) {
+			navdataManager = new NavDataManager1(inetaddr, manager);
+		} else if (ardroneVersion == ARDroneVersion.ARDRONE2) {
+			navdataManager = new NavDataManager2(inetaddr, manager);
+		} else {
+			error("Cannot create NavData manager", this);
+			error("Maybe this is not AR.Drone?", this);
+			return false;
+		}
+		navdataManager.setAttitudeListener(new AttitudeListener() {
+			@Override
+			public void attitudeUpdated(float pitch, float roll, float yaw,
+					int altitude) {
+				if (attitudeListener != null) {
+					attitudeListener
+							.attitudeUpdated(pitch, roll, yaw, altitude);
+				}
+			}
+		});
+		navdataManager.setBatteryListener(new BatteryListener() {
+			@Override
+			public void batteryLevelChanged(int percentage) {
+				if (batteryListener != null) {
+					batteryListener.batteryLevelChanged(percentage);
+				}
+			}
+		});
+		navdataManager.setStateListener(new StateListener() {
+			@Override
+			public void stateChanged(DroneState state) {
+				if (stateListener != null) {
+					stateListener.stateChanged(state);
+				}
+			}
+		});
+		navdataManager.setVelocityListener(new VelocityListener() {
+			@Override
+			public void velocityChanged(float vx, float vy, float vz) {
+				if (velocityListener != null) {
+					velocityListener.velocityChanged(vx, vy, vz);
+				}
+			}
+		});
+
+		return navdataManager.connect(ARDroneConstants.NAV_PORT);
+	}
+
+	@Override
+	public void disconnect() {
+		stop();
+		landing();
+		manager.close();
+		if (videoManager != null)
+			videoManager.close();
+		if (navdataManager != null)
+			navdataManager.close();
+	}
+
+	@Override
+	public void start() {
+		if (manager != null)
+			new Thread(manager).start();
+		if (videoManager != null)
+			new Thread(videoManager).start();
+		if (navdataManager != null)
+			new Thread(navdataManager).start();
+	}
+
+	@Override
+	public void setHorizontalCamera() {
+		if (manager != null)
+			manager.setHorizontalCamera();
+	}
+
+	@Override
+	public void setVerticalCamera() {
+		if (manager != null)
+			manager.setVerticalCamera();
+	}
+
+	@Override
+	public void setHorizontalCameraWithVertical() {
+		if (manager != null)
+			manager.setHorizontalCameraWithVertical();
+	}
+
+	@Override
+	public void setVerticalCameraWithHorizontal() {
+		if (manager != null)
+			manager.setVerticalCameraWithHorizontal();
+	}
+
+	@Override
+	public void toggleCamera() {
+		if (manager != null)
+			manager.toggleCamera();
+	}
+
+	@Override
+	public void landing() {
+		if (manager != null)
+			manager.landing();
+	}
+
+	@Override
+	public void takeOff() {
+		if (manager != null)
+			manager.takeOff();
+	}
+
+	@Override
+	public void reset() {
+		if (manager != null)
+			manager.reset();
+	}
+
+	@Override
+	public void forward() {
+		if (manager != null)
+			manager.forward();
+	}
+
+	@Override
+	public void forward(int speed) {
+		if (manager != null)
+			manager.forward(speed);
+	}
+
+	@Override
+	public void backward() {
+		if (manager != null)
+			manager.backward();
+	}
+
+	@Override
+	public void backward(int speed) {
+		if (manager != null)
+			manager.backward(speed);
+	}
+
+	@Override
+	public void spinRight() {
+		if (manager != null)
+			manager.spinRight();
+	}
+
+	@Override
+	public void spinRight(int speed) {
+		if (manager != null)
+			manager.spinRight(speed);
+	}
+
+	@Override
+	public void spinLeft() {
+		if (manager != null)
+			manager.spinLeft();
+	}
+
+	@Override
+	public void spinLeft(int speed) {
+		if (manager != null)
+			manager.spinLeft(speed);
+	}
+
+	@Override
+	public void up() {
+		if (manager != null)
+			manager.up();
+	}
+
+	@Override
+	public void up(int speed) {
+		if (manager != null)
+			manager.up(speed);
+	}
+
+	@Override
+	public void down() {
+		if (manager != null)
+			manager.down();
+	}
+
+	@Override
+	public void down(int speed) {
+		if (manager != null)
+			manager.down(speed);
+	}
+
+	@Override
+	public void goRight() {
+		if (manager != null)
+			manager.goRight();
+	}
+
+	@Override
+	public void goRight(int speed) {
+		if (manager != null)
+			manager.goRight(speed);
+	}
+
+	@Override
+	public void goLeft() {
+		if (manager != null)
+			manager.goLeft();
+	}
+
+	@Override
+	public void goLeft(int speed) {
+		if (manager != null)
+			manager.goLeft(speed);
+	}
+
+	@Override
+	public void setSpeed(int speed) {
+		if (manager != null)
+			manager.setSpeed(speed);
+	}
+
+	@Override
+	public void stop() {
+		if (manager != null)
+			manager.stop();
+	}
+
+	/**
+	 * 0.01-1.0 -> 1-100%
+	 * 
+	 * @return 1-100%
+	 */
+	@Override
+	public int getSpeed() {
+		if (manager == null)
+			return -1;
+		return manager.getSpeed();
+	}
+
+	@Override
+	public void setMaxAltitude(int altitude) {
+		if (manager != null)
+			manager.setMaxAltitude(altitude);
+	}
+
+	@Override
+	public void setMinAltitude(int altitude) {
+		if (manager != null)
+			manager.setMinAltitude(altitude);
+	}
+
+	@Override
+	public void move3D(int speedX, int speedY, int speedZ, int speedSpin) {
+		if (manager != null)
+			manager.move3D(speedX, speedY, speedZ, speedSpin);
+	}
+
+	// update listeners
+	public void addImageUpdateListener(ImageListener imageListener) {
+		this.imageListener = imageListener;
+	}
+
+	public void addAttitudeUpdateListener(AttitudeListener attitudeListener) {
+		this.attitudeListener = attitudeListener;
+	}
+
+	public void addBatteryUpdateListener(BatteryListener batteryListener) {
+		this.batteryListener = batteryListener;
+	}
+
+	public void addStateUpdateListener(StateListener stateListener) {
+		this.stateListener = stateListener;
+	}
+
+	public void addVelocityUpdateListener(VelocityListener velocityListener) {
+		this.velocityListener = velocityListener;
+	}
+
+	// remove listeners
+	public void removeImageUpdateListener() {
+		imageListener = null;
+	}
+
+	public void removeAttitudeUpdateListener() {
+		attitudeListener = null;
+	}
+
+	public void removeBatteryUpdateListener() {
+		batteryListener = null;
+	}
+
+	public void removeStateUpdateListener() {
+		stateListener = null;
+	}
+
+	public void removeVelocityUpdateListener() {
+		velocityListener = null;
+	}
+
+	/**
+	 * print error message
+	 * 
+	 * @param message
+	 * @param obj
+	 */
+	public static void error(String message, Object obj) {
+		System.err.println("[" + obj.getClass() + "] " + message);
+	}
+
+	private InetAddress getInetAddress(String ipaddr) {
+		InetAddress inetaddr = null;
+		StringTokenizer st = new StringTokenizer(ipaddr, ".");
+		byte[] ipBytes = new byte[4];
+		if (st.countTokens() == 4) {
+			for (int i = 0; i < 4; i++) {
+				ipBytes[i] = (byte) Integer.parseInt(st.nextToken());
+			}
+		} else {
+			error("Incorrect IP address format: " + ipaddr, this);
+			return null;
+		}
+		try {
+			inetaddr = InetAddress.getByAddress(ipBytes);
+		} catch (UnknownHostException e) {
+			e.printStackTrace();
+		}
+		return inetaddr;
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/ARDroneInterface.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+
+package com.shigeodayo.ardrone;
+
+public interface ARDroneInterface {
+
+	
+	// connection
+	public boolean connect();
+
+	public boolean connectVideo();
+
+	public boolean connectNav();
+
+	public void disconnect();
+
+	public void start();
+
+	// camera
+	public void setHorizontalCamera();// setFrontCameraStreaming()
+
+	public void setVerticalCamera();// setBellyCameraStreaming()
+
+	public void setHorizontalCameraWithVertical();// setFrontCameraWithSmallBellyStreaming()
+
+	public void setVerticalCameraWithHorizontal();// setBellyCameraWithSmallFrontStreaming()
+
+	public void toggleCamera();
+
+	// control command
+	public void landing();
+
+	public void takeOff();
+
+	public void reset();
+
+	public void forward();
+
+	public void forward(int speed);
+
+	public void backward();
+
+	public void backward(int speed);
+
+	public void spinRight();
+
+	public void spinRight(int speed);
+
+	public void spinLeft();
+
+	public void spinLeft(int speed);
+
+	public void up();
+
+	public void up(int speed);
+
+	public void down();
+
+	public void down(int speed);
+
+	public void goRight();
+
+	public void goRight(int speed);
+
+	public void goLeft();
+
+	public void goLeft(int speed);
+
+	public void stop();
+
+	public void move3D(int speedX, int speedY, int speedZ, int speedSpin);
+
+	// speed
+	public int getSpeed();
+	public void setSpeed(int speed);
+
+	// set max altitude
+	public void setMaxAltitude(int altitude);
+	// set min altitude
+	public void setMinAltitude(int altitude);
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/command/CommandManager.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.command;
+
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.InetAddress;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+import java.nio.IntBuffer;
+
+import com.shigeodayo.ardrone.manager.AbstractManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+public abstract class CommandManager extends AbstractManager {
+
+	protected static final String CR = "\r";
+	protected static final String SEQ = "$SEQ$";
+
+	private static int seq = 1;
+
+	private FloatBuffer fb = null;
+	private IntBuffer ib = null;
+
+	private boolean landing = true;
+	
+	private boolean continuance = false;
+	private String command = null;
+
+	/** speed */
+	private float speed = 0.05f; // 0.01f - 1.0f
+
+	protected String VIDEO_CODEC;
+
+	public CommandManager(InetAddress inetaddr) {
+		this.inetaddr = inetaddr;
+
+		ByteBuffer bb = ByteBuffer.allocate(4);
+		fb = bb.asFloatBuffer();
+		ib = bb.asIntBuffer();
+	}
+
+	public void setHorizontalCamera() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"0\"";
+		continuance = false;
+	}
+
+	public void setVerticalCamera() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"1\"";
+		continuance = false;
+	}
+
+	public void setHorizontalCameraWithVertical() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"2\"";
+		continuance = false;
+	}
+
+	public void setVerticalCameraWithHorizontal() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"3\"";
+		continuance = false;
+	}
+
+	public void toggleCamera() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"4\"";
+		continuance = false;
+	}
+
+	public void landing() {
+		command = "AT*REF=" + SEQ + ",290717696";
+		continuance = false;
+		landing = true;
+		//System.out.println("landing");
+	}
+
+	public void takeOff() {
+		sendCommand("AT*FTRIM=" + SEQ);
+		command = "AT*REF=" + SEQ + ",290718208";
+		continuance = false;
+		landing = false;
+		//System.out.println("take off");
+	}
+
+	public void reset() {
+		command = "AT*REF=" + SEQ + ",290717952";
+		continuance = true;
+		landing = true;
+	}
+
+	public void forward() {
+		command = "AT*PCMD=" + SEQ + ",1,0," + intOfFloat(-speed) + ",0,0"
+				+ "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void forward(int speed) {
+		setSpeed(speed);
+		forward();
+	}
+
+	public void backward() {
+		command = "AT*PCMD=" + SEQ + ",1,0," + intOfFloat(speed) + ",0,0"
+				+ "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void backward(int speed) {
+		setSpeed(speed);
+		backward();
+	}
+
+	public void spinRight() {
+		command = "AT*PCMD=" + SEQ + ",1,0,0,0," + intOfFloat(speed) + "\r"
+				+ "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void spinRight(int speed) {
+		setSpeed(speed);
+		spinRight();
+	}
+
+	public void spinLeft() {
+		command = "AT*PCMD=" + SEQ + ",1,0,0,0," + intOfFloat(-speed) + "\r"
+				+ "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void spinLeft(int speed) {
+		setSpeed(speed);
+		spinLeft();
+	}
+
+	public void up() {
+		command = "AT*PCMD=" + SEQ + ",1," + intOfFloat(0) + ","
+				+ intOfFloat(0) + "," + intOfFloat(speed) + "," + intOfFloat(0)
+				+ "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void up(int speed) {
+		setSpeed(speed);
+		up();
+	}
+
+	public void down() {
+		command = "AT*PCMD=" + SEQ + ",1," + intOfFloat(0) + ","
+				+ intOfFloat(0) + "," + intOfFloat(-speed) + ","
+				+ intOfFloat(0) + "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void down(int speed) {
+		setSpeed(speed);
+		down();
+	}
+
+	public void goRight() {
+		command = "AT*PCMD=" + SEQ + ",1," + intOfFloat(speed) + ",0,0,0"
+				+ "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void goRight(int speed) {
+		setSpeed(speed);
+		goRight();
+	}
+
+	public void goLeft() {
+		command = "AT*PCMD=" + SEQ + ",1," + intOfFloat(-speed) + ",0,0,0"
+				+ "\r" + "AT*REF=" + SEQ + ",290718208";
+		continuance = true;
+	}
+
+	public void goLeft(int speed) {
+		setSpeed(speed);
+		goLeft();
+	}
+
+	public void stop() {
+		command = "AT*PCMD=" + SEQ + ",1,0,0,0,0";
+		continuance = true;
+	}
+
+	public void setSpeed(int speed) {
+		if (speed > 100)
+			speed = 100;
+		else if (speed < 1)
+			speed = 1;
+
+		this.speed = (float) (speed / 100.0);
+	}
+
+	public void enableVideoData() {
+		command = "AT*CONFIG=" + SEQ + ",\"general:video_enable\",\"TRUE\""
+				+ CR + "AT*FTRIM=" + SEQ;
+		continuance = false;
+	}
+
+	public void enableDemoData() {
+		command = "AT*CONFIG=" + SEQ + ",\"general:navdata_demo\",\"TRUE\""
+				+ CR + "AT*FTRIM=" + SEQ;
+		continuance = false;
+	}
+
+	public void disableBootStrap() {
+		command = "AT*CONFIG_IDS=" + SEQ + ",\"" + ARDroneConstants.SESSION_ID
+				+ "\",\"" + ARDroneConstants.PROFILE_ID + "\",\""
+				+ ARDroneConstants.APPLICATION_ID + "\"" + CR;
+	}
+
+	public void sendControlAck() {
+		command = "AT*CTRL=" + SEQ + ",0";
+		continuance = false;
+	}
+
+	public int getSpeed() {
+		return (int) (speed * 100);
+	}
+
+	public void disableAutomaticVideoBitrate() {
+		command = "AT*CONFIG=" + SEQ + ",\"video:bitrate_ctrl_mode\",\"0\"";
+		continuance = false;
+	}
+
+	public void setMaxAltitude(int altitude) {
+		command = "AT*CONFIG=" + SEQ + ",\"control:altitude_max\",\""
+				+ altitude + "\"";
+		continuance = false;
+	}
+
+	public void setMinAltitude(int altitude) {
+		command = "AT*CONFIG=" + SEQ + ",\"control:altitude_min\",\""
+				+ altitude + "\"";
+		continuance = false;
+	}
+
+	/*
+	 * Thank you Tarqunio !!
+	 */
+	public void move3D(int speedX, int speedY, int speedZ, int speedSpin) {
+		if (speedX > 100)
+			speedX = 100;
+		else if (speedX < -100)
+			speedX = -100;
+		if (speedY > 100)
+			speedY = 100;
+		else if (speedY < -100)
+			speedY = -100;
+		if (speedZ > 100)
+			speedZ = 100;
+		else if (speedZ < -100)
+			speedZ = -100;
+
+		command = "AT*PCMD=" + SEQ + ",1," + intOfFloat(-speedY / 100.0f) + ","
+				+ intOfFloat(-speedX / 100.0f) + ","
+				+ intOfFloat(-speedZ / 100.0f) + ","
+				+ intOfFloat(-speedSpin / 100.0f) + "\r" + "AT*REF=" + SEQ
+				+ ",290718208";
+		continuance = true;
+	}
+
+	@Override
+	public void run() {
+		initializeDrone();
+		while (true) {
+			if (this.command != null) {
+				// sendCommand();
+				sendCommand(this.command);
+				if (!continuance) {
+					command = null;
+				}
+			} else {
+				if (landing) {
+					sendCommand("AT*PCMD=" + SEQ + ",1,0,0,0,0" + CR
+							+ "AT*REF=" + SEQ + ",290717696");
+				} else {
+					sendCommand("AT*PCMD=" + SEQ + ",1,0,0,0,0" + CR
+							+ "AT*REF=" + SEQ + ",290718208");
+				}
+			}
+
+			try {
+				Thread.sleep(20); // < 50ms
+			} catch (InterruptedException e) {
+				e.printStackTrace();
+			}
+
+			if (seq % 5 == 0) { // < 2000ms
+				sendCommand("AT*COMWDG=" + SEQ);
+			}
+
+		}
+	}
+
+	protected abstract void initializeDrone();
+
+	/*
+	 * private void initializeDrone() { sendCommand("AT*CONFIG=" + SEQ +
+	 * ",\"general:navdata_demo\",\"TRUE\"" + CR + "AT*FTRIM=" + SEQ); // 1
+	 * sendCommand("AT*PMODE=" + SEQ + ",2" + CR + "AT*MISC=" + SEQ +
+	 * ",2,20,2000,3000" + CR + "AT*FTRIM=" + SEQ + CR + "AT*REF=" + SEQ +
+	 * ",290717696"); // 2-5 sendCommand("AT*PCMD=" + SEQ + ",1,0,0,0,0" + CR +
+	 * "AT*REF=" + SEQ + ",290717696" + CR + "AT*COMWDG=" + SEQ); // 6-8
+	 * sendCommand("AT*PCMD=" + SEQ + ",1,0,0,0,0" + CR + "AT*REF=" + SEQ +
+	 * ",290717696" + CR + "AT*COMWDG=" + SEQ); // 6-8 sendCommand("AT*FTRIM=" +
+	 * SEQ); //System.out.println("Initialize completed!"); }
+	 */
+
+	/*
+	 * Thank you Dirk !! 
+	 */
+	protected synchronized void sendCommand(String command) {
+		int seqIndex = -1;
+		while ((seqIndex = command.indexOf(SEQ)) != -1)
+			command = command.substring(0, seqIndex) + (seq++)
+					+ command.substring(seqIndex + SEQ.length());
+
+		byte[] buffer = (command + CR).getBytes();
+		//System.out.println(command);
+		DatagramPacket packet = new DatagramPacket(buffer, buffer.length,
+				inetaddr, ARDroneConstants.PORT);
+
+		try {
+			socket.send(packet);
+			 //Thread.sleep(20); // < 50ms
+		} catch (IOException e) {
+			e.printStackTrace();
+		} /*catch (InterruptedException e) {
+			e.printStackTrace();
+		}*/
+	}
+
+	private int intOfFloat(float f) {
+		fb.put(0, f);
+		return ib.get(0);
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/command/CommandManager1.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.command;
+
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+/**
+ * CommandManager for AR.Drone 1.0
+ * 
+ * @author shigeo
+ * 
+ */
+public class CommandManager1 extends CommandManager {
+
+	public CommandManager1(InetAddress inetaddr) {
+		this(inetaddr, false);
+	}
+
+	public CommandManager1(InetAddress inetaddr, boolean useHighRezVideoStreaming) {
+		super(inetaddr);
+		if (useHighRezVideoStreaming) // high resolution version is not implemented yet
+			VIDEO_CODEC = ARDroneConstants.VIDEO_CODEC_H264;
+		else
+			VIDEO_CODEC = ARDroneConstants.VIDEO_CODEC_UVLC;
+		
+
+		//System.out.println("command manager 1");
+	}
+
+	@Override
+	protected void initializeDrone() {
+		try {
+			sendCommand("AT*CONFIG=" + SEQ +
+					  ",\"general:navdata_demo\",\"TRUE\"" + CR + "AT*FTRIM=" + SEQ);
+					  
+			Thread.sleep(20);
+			
+			sendCommand("AT*PMODE=" + SEQ + ",2" + CR);
+
+			Thread.sleep(20);
+
+			sendCommand("AT*MISC=" + SEQ + ",2,20,2000,3000" + CR);
+
+			Thread.sleep(20);
+
+			// enable video
+			sendCommand("AT*CONFIG=" + SEQ
+					+ ",\"general:video_enable\",\"TRUE\"" + CR);
+
+			Thread.sleep(20);
+
+			// fix bit rate
+			sendCommand("AT*CONFIG=" + SEQ
+					+ ",\"video:bitrate_ctrl_mode\",\"0\"" + CR);
+			
+			Thread.sleep(20);
+
+			// video codec
+			sendCommand("AT*CONFIG=" + SEQ + ",\"video:video_codec\",\""
+					+ VIDEO_CODEC + "\"" + CR);
+
+			Thread.sleep(20);
+
+			// set front camera
+			sendCommand("AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"0\""
+					+ CR);
+
+			Thread.sleep(20);
+
+			// trim
+			sendCommand("AT*FTRIM=" + SEQ + CR);
+
+			Thread.sleep(20);
+
+		} catch (InterruptedException e) {
+			e.printStackTrace();
+		}
+		
+		System.out.println("Initialize AR.Drone 1.0 !!");
+	}
+}

src/com/shigeodayo/ardrone/command/CommandManager2.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.command;
+
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+/**
+ * CommandManager for AR.Drone 2.0
+ * 
+ * @author shigeo
+ * 
+ */
+public class CommandManager2 extends CommandManager {
+
+	private final String SESSION_ID = ARDroneConstants.SESSION_ID;
+	private final String PROFILE_ID = ARDroneConstants.PROFILE_ID;
+	private final String APPLICATION_ID = ARDroneConstants.APPLICATION_ID;
+
+	public CommandManager2(InetAddress inetaddr) {
+		this(inetaddr, false);
+	}
+
+	public CommandManager2(InetAddress inetaddr,
+			boolean useHighRezVideoStreaming) {
+		super(inetaddr);
+		if (useHighRezVideoStreaming) // high resolution version is not implemented yet
+			VIDEO_CODEC = ARDroneConstants.VIDEO_CODEC_720P;
+		else
+			VIDEO_CODEC = ARDroneConstants.VIDEO_CODEC_360P;
+
+		// System.out.println("command manager 2");
+	}
+
+	@Override
+	protected void initializeDrone() {
+		try {
+
+			sendCommand("AT*CONFIG=" + SEQ
+					+ ",\"general:navdata_demo\",\"TRUE\"" + CR + "AT*FTRIM="
+					+ SEQ); // 1
+
+			Thread.sleep(20);
+
+			sendCommand("AT*PMODE=" + SEQ + ",2" + CR);
+
+			Thread.sleep(20);
+
+			sendCommand("AT*MISC=" + SEQ + ",2,20,2000,3000" + CR);
+
+			Thread.sleep(20);
+
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ + ",\"custom:session_id\",\""
+					+ SESSION_ID + "\"" + CR);
+
+			Thread.sleep(20);
+
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ + ",\"custom:profile_id\",\""
+					+ PROFILE_ID + "\"" + CR);
+
+			Thread.sleep(20);
+
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ + ",\"custom:application_id\",\""
+					+ APPLICATION_ID + "\"" + CR);
+
+			Thread.sleep(20);
+
+			// enable video
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ
+					+ ",\"general:video_enable\",\"TRUE\"" + CR);
+
+			Thread.sleep(20);
+
+			// fix bit rate
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ
+					+ ",\"video:bitrate_ctrl_mode\",\"0\"" + CR);
+
+			Thread.sleep(20);
+
+			// video codec
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ + ",\"video:video_codec\"," + "\""
+					+ VIDEO_CODEC + "\"" + CR);
+
+			Thread.sleep(20);
+
+			// set front camera
+			sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+					+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+			sendCommand("AT*CONFIG=" + SEQ + ",\"video:video_channel\",\"0\""
+					+ CR);
+
+			Thread.sleep(20);
+
+			// trim
+			sendCommand("AT*FTRIM=" + SEQ + CR);
+
+			Thread.sleep(20);
+
+		} catch (InterruptedException e) {
+			e.printStackTrace();
+		}
+
+		System.out.println("Initialize AR.Drone 2.0 !!");
+
+	}
+
+	@Override
+	public void setHorizontalCamera() {
+		sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+				+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+		super.setHorizontalCamera();
+	}
+
+	@Override
+	public void setVerticalCamera() {
+		sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+				+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+		super.setVerticalCamera();
+	}
+
+	@Override
+	public void setHorizontalCameraWithVertical() {
+	}
+
+	@Override
+	public void setVerticalCameraWithHorizontal() {
+	}
+
+	@Override
+	public void toggleCamera() {
+		sendCommand("AT*CONFIG_IDS=" + SEQ + ",\"" + SESSION_ID + "\",\""
+				+ PROFILE_ID + "\",\"" + APPLICATION_ID + "\"" + CR);
+		super.toggleCamera();
+	}
+
+}

src/com/shigeodayo/ardrone/manager/AbstractManager.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.manager;
+
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+
+public abstract class AbstractManager implements Runnable {
+
+	protected InetAddress inetaddr = null;
+	protected DatagramSocket socket = null;
+
+	public boolean connect(int port) {
+		try {
+			socket = new DatagramSocket(port);
+			socket.setSoTimeout(3000);
+		} catch (SocketException e) {
+			e.printStackTrace();
+			return false;
+		}
+		return true;
+	}
+
+	public void close() {
+		socket.close();
+	}
+
+	protected void ticklePort(int port) {
+		byte[] buf = { 0x01, 0x00, 0x00, 0x00 };
+		DatagramPacket packet = new DatagramPacket(buf, buf.length, inetaddr,
+				port);
+		try {
+			socket.send(packet);
+		} catch (IOException e) {
+			e.printStackTrace();
+		}
+	}
+}

src/com/shigeodayo/ardrone/navdata/AttitudeListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package com.shigeodayo.ardrone.navdata;
+
+public interface AttitudeListener {
+	void attitudeUpdated(float pitch, float roll, float yaw, int altitude);
+}

src/com/shigeodayo/ardrone/navdata/BatteryListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package com.shigeodayo.ardrone.navdata;
+
+public interface BatteryListener {
+	void batteryLevelChanged(int percentage);
+}

src/com/shigeodayo/ardrone/navdata/DetectType.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.navdata;
+
+public class DetectType {
+
+	public DetectType() {
+
+	}
+
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/navdata/DetectionDataListener.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.navdata;
+
+import java.awt.Dimension;
+import java.awt.Point;
+
+public interface DetectionDataListener {
+	void detectionDataChanged(DetectType type, Point pos, Dimension size,
+			int distance);
+}

src/com/shigeodayo/ardrone/navdata/DroneState.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package com.shigeodayo.ardrone.navdata;
+
+public class DroneState {
+	private final int bits;
+
+	public DroneState(int bits) {
+		this.bits = bits;
+	}
+
+	public String toString() {
+		return "DroneState(" + Integer.toHexString(bits) + ")";
+	}
+
+	public boolean equals(Object o) {
+		if (o == null || o.getClass() != getClass())
+			return false;
+		return bits == ((DroneState) o).bits;
+	}
+
+	public int hashCode() {
+		return 31 * bits;
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/navdata/NavDataException.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package com.shigeodayo.ardrone.navdata;
+
+public class NavDataException extends Exception {
+
+	private static final long serialVersionUID = 1311407045280371188L;
+
+	public NavDataException(String message) {
+		super(message);
+	}
+}

src/com/shigeodayo/ardrone/navdata/NavDataManager.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.navdata;
+
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.InetAddress;
+import java.nio.ByteBuffer;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.manager.AbstractManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+public abstract class NavDataManager extends AbstractManager {
+
+	protected CommandManager manager = null;
+
+	// listeners
+	private AttitudeListener attitudeListener = null;
+	private StateListener stateListener = null;
+	private VelocityListener velocityListener = null;
+	private BatteryListener batteryListener = null;
+
+	public NavDataManager(InetAddress inetaddr, CommandManager manager) {
+		this.inetaddr = inetaddr;
+		this.manager = manager;
+	}
+
+	public void setAttitudeListener(AttitudeListener attitudeListener) {
+		this.attitudeListener = attitudeListener;
+	}
+
+	public void setBatteryListener(BatteryListener batteryListener) {
+		this.batteryListener = batteryListener;
+	}
+
+	public void setStateListener(StateListener stateListener) {
+		this.stateListener = stateListener;
+	}
+
+	public void setVelocityListener(VelocityListener velocityListener) {
+		this.velocityListener = velocityListener;
+	}
+
+	@Override
+	public void run() {
+		initializeDrone();
+		
+		NavDataParser parser = new NavDataParser();
+
+		parser.setAttitudeListener(attitudeListener);
+		parser.setBatteryListener(batteryListener);
+		parser.setStateListener(stateListener);
+		parser.setVelocityListener(velocityListener);
+
+		while (true) {
+			try {
+				ticklePort(ARDroneConstants.NAV_PORT);
+				DatagramPacket packet = new DatagramPacket(new byte[1024],
+						1024, inetaddr, ARDroneConstants.NAV_PORT);
+
+				socket.receive(packet);
+
+				ByteBuffer buffer = ByteBuffer.wrap(packet.getData(), 0,
+						packet.getLength());
+
+				parser.parseNavData(buffer);
+			} catch (IOException e) {
+				e.printStackTrace();
+			} catch (NavDataException e) {
+				e.printStackTrace();
+			}
+		}
+	}
+	
+	protected abstract void initializeDrone();
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/navdata/NavDataManager1.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.navdata;
+
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+public class NavDataManager1 extends NavDataManager {
+
+	public NavDataManager1(InetAddress inetaddr, CommandManager manager) {
+		super(inetaddr, manager);
+		
+		//System.out.println("navdata manager 1");
+	}
+
+	@Override
+	protected void initializeDrone() {
+		ticklePort(ARDroneConstants.NAV_PORT);
+		manager.enableDemoData();
+		ticklePort(ARDroneConstants.NAV_PORT);
+		manager.sendControlAck();		
+	}
+}

src/com/shigeodayo/ardrone/navdata/NavDataManager2.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.navdata;
+
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+public class NavDataManager2 extends NavDataManager {
+
+	public NavDataManager2(InetAddress inetaddr, CommandManager manager) {
+		super(inetaddr, manager);
+		
+		//System.out.println("navdata manager 2");
+	}
+
+	@Override
+	protected void initializeDrone() {
+		ticklePort(ARDroneConstants.NAV_PORT);
+		//manager.disableBootStrap();
+		manager.enableDemoData();
+		ticklePort(ARDroneConstants.NAV_PORT);
+		manager.sendControlAck();		
+	}	
+}

src/com/shigeodayo/ardrone/navdata/NavDataParser.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+
+package com.shigeodayo.ardrone.navdata;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+public class NavDataParser {
+	private AttitudeListener attitudeListener;
+	private StateListener stateListener;
+	private VelocityListener velocityListener;
+	private BatteryListener batteryListener;
+
+	long lastSequenceNumber = 1;
+
+	// set listeners
+	public void setBatteryListener(BatteryListener batteryListener) {
+		this.batteryListener = batteryListener;
+	}
+
+	public void setAttitudeListener(AttitudeListener attitudeListener) {
+		this.attitudeListener = attitudeListener;
+	}
+
+	public void setStateListener(StateListener stateListener) {
+		this.stateListener = stateListener;
+	}
+
+	public void setVelocityListener(VelocityListener velocityListener) {
+		this.velocityListener = velocityListener;
+	}
+
+	public void parseNavData(ByteBuffer buffer) throws NavDataException {
+		buffer.order(ByteOrder.LITTLE_ENDIAN);
+		int magic = buffer.getInt();
+		// System.out.printf("%02x\n", magic);
+		requireEquals("Magic must be correct", 0x55667788, magic);
+
+		int state = buffer.getInt();
+		long sequence = buffer.getInt() & 0xFFFFFFFFL;
+		@SuppressWarnings("unused")
+		int vision = buffer.getInt();
+
+		if (sequence <= lastSequenceNumber && sequence != 1) {
+			return;
+		}
+		lastSequenceNumber = sequence;
+
+		if (stateListener != null) {
+			stateListener.stateChanged(new DroneState(state));
+		}
+
+		while (buffer.position() < buffer.limit()) {
+			int tag = buffer.getShort() & 0xFFFF;
+			int payloadSize = (buffer.getShort() & 0xFFFF) - 4;
+			ByteBuffer optionData = buffer.slice().order(
+					ByteOrder.LITTLE_ENDIAN);
+			optionData.limit(payloadSize);
+			buffer.position(buffer.position() + payloadSize);
+
+			dispatch(tag, optionData);
+		}
+	}
+
+	private void dispatch(int tag, ByteBuffer optionData) {
+		switch (tag) {
+		case 0:
+			processNavDataDemo(optionData);
+			break;
+		}
+	}
+
+	private void processNavDataDemo(ByteBuffer optionData) {
+		@SuppressWarnings("unused")
+		int controlState = optionData.getInt();
+		int batteryPercentage = optionData.getInt();
+
+		float theta = optionData.getFloat() / 1000;
+		float phi = optionData.getFloat() / 1000;
+		float psi = optionData.getFloat() / 1000;
+
+		int altitude = optionData.getInt();
+
+		float vx = optionData.getFloat();
+		float vy = optionData.getFloat();
+		float vz = optionData.getFloat();
+
+		if (batteryListener != null) {
+			batteryListener.batteryLevelChanged(batteryPercentage);
+		}
+
+		if (attitudeListener != null) {
+			attitudeListener.attitudeUpdated(theta, phi, psi, altitude);
+			// System.out.println("update in parser");
+		}
+
+		if (velocityListener != null) {
+			velocityListener.velocityChanged(vx, vy, vz);
+		}
+	}
+
+	private void requireEquals(String message, int expected, int actual)
+			throws NavDataException {
+		if (expected != actual) {
+			throw new NavDataException(message + " : expected " + expected
+					+ ", was " + actual);
+		}
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/navdata/StateListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+
+package com.shigeodayo.ardrone.navdata;
+
+public interface StateListener {
+	void stateChanged(DroneState state);
+}

src/com/shigeodayo/ardrone/navdata/VelocityListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package com.shigeodayo.ardrone.navdata;
+
+public interface VelocityListener {
+	void velocityChanged(float vx, float vy, float vz);
+}

src/com/shigeodayo/ardrone/processing/ARDroneForP5.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.processing;
+
+import java.awt.Graphics2D;
+//import java.awt.Point;
+import java.awt.image.BufferedImage;
+//import java.awt.image.DataBufferInt;
+//import java.awt.image.Raster;
+//import java.awt.image.WritableRaster;
+
+import processing.core.PConstants;
+import processing.core.PImage;
+
+import com.shigeodayo.ardrone.ARDrone;
+import com.shigeodayo.ardrone.navdata.AttitudeListener;
+import com.shigeodayo.ardrone.navdata.BatteryListener;
+import com.shigeodayo.ardrone.navdata.DroneState;
+import com.shigeodayo.ardrone.navdata.StateListener;
+import com.shigeodayo.ardrone.navdata.VelocityListener;
+import com.shigeodayo.ardrone.utils.ARDroneVersion;
+import com.shigeodayo.ardrone.video.ImageListener;
+
+/**
+ * AR.Drone library for Processing
+ * 
+ * @author shigeo
+ * 
+ */
+public class ARDroneForP5 extends ARDrone implements ImageListener,
+		AttitudeListener, BatteryListener, StateListener, VelocityListener {
+
+	private BufferedImage videoImage = null;
+
+	private float pitch = 0.0f;
+	private float roll = 0.0f;
+	private float yaw = 0.0f;
+	private float altitude = 0.0f;
+
+	private int battery = 0;
+
+	private DroneState state = null;
+
+	private float vx = 0.0f;
+	private float vy = 0.0f;
+	private float[] velocity = new float[2];
+
+	private PImage pimg = null;
+	//private WritableRaster wr = null;
+
+	/** constructor */
+	public ARDroneForP5() {
+		super();
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ipaddr
+	 */
+	public ARDroneForP5(String ipaddr) {
+		super(ipaddr);
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ardroneType
+	 */
+	public ARDroneForP5(ARDroneVersion ardroneType) {
+		super(ardroneType);
+	}
+
+	/**
+	 * constructor
+	 * 
+	 * @param ipaddr
+	 * @param ardroneType
+	 */
+	public ARDroneForP5(String ipaddr, ARDroneVersion ardroneType) {
+		super(ipaddr, ardroneType);
+	}
+
+	@Override
+	public boolean connectVideo() {
+		addImageUpdateListener(this);
+		// pimg = new PImage(320, 240);
+		return super.connectVideo();
+	}
+
+	@Override
+	public boolean connectNav() {
+		addAttitudeUpdateListener(this);
+		addBatteryUpdateListener(this);
+		addStateUpdateListener(this);
+		addVelocityUpdateListener(this);
+		return super.connectNav();
+	}
+
+	@Override
+	public void imageUpdated(BufferedImage image) {
+		this.videoImage = image;
+	}
+
+	@Override
+	public void velocityChanged(float vx, float vy, float vz) {
+		this.vx = vx;
+		this.vy = vy;
+		velocity[0] = vx;
+		velocity[1] = vy;
+	}
+
+	@Override
+	public void stateChanged(DroneState state) {
+		this.state = state;
+	}
+
+	@Override
+	public void batteryLevelChanged(int percentage) {
+		this.battery = percentage;
+	}
+
+	@Override
+	public void attitudeUpdated(float pitch, float roll, float yaw, int altitude) {
+		this.pitch = pitch;
+		this.yaw = yaw;
+		this.roll = roll;
+		this.altitude = altitude;
+	}
+
+	public void printARDroneInfo() {
+		System.out
+				.println("--------------------------------------------------------------------");
+		System.out.println("Attitude: pitch=" + pitch + " roll=" + roll
+				+ " yaw=" + yaw + " altitude=" + altitude);
+		System.out.println("Battery: " + battery + "%");
+		System.out.println("Velocity: vx=" + vx + " vy=" + vy);
+		System.out
+				.println("--------------------------------------------------------------------");
+	}
+
+	public PImage getVideoImage(boolean autoResize) {
+		if (videoImage == null)
+			return null;
+		if (autoResize) {
+			if (videoImage.getWidth() == 176) {
+				return convertToPImage(resize(videoImage, 320, 240));
+			}
+		}
+		return convertToPImage(videoImage);
+
+	}
+
+	public float getPitch() {
+		return pitch;
+	}
+
+	public float getRoll() {
+		return roll;
+	}
+
+	public float getYaw() {
+		return yaw;
+	}
+
+	public float getAltitude() {
+		return altitude;
+	}
+
+	public float getVelocityX() {
+		return vx;
+	}
+
+	public float getVelocityY() {
+		return vy;
+	}
+
+	public float[] getVelocity() {
+		return velocity;
+	}
+
+	public int getBatteryPercentage() {
+		return battery;
+	}
+
+	private PImage convertToPImage(BufferedImage bufImg) {
+		if (bufImg == null)
+			return null;
+		try {
+
+			/*
+			 * if (pimg == null) { System.out.println("new pimage"); //pimg =
+			 * new PImage(bufImg); //pimg = new PImage(); pimg = new PImage(320,
+			 * 240, PConstants.ARGB); DataBufferInt dbi = new
+			 * DataBufferInt(pimg.pixels, pimg.pixels.length); wr =
+			 * Raster.createWritableRaster(bufImg.getSampleModel(), dbi, new
+			 * Point(0, 0)); } else { System.out.println("update pimage");
+			 * bufImg.copyData(wr); pimg.updatePixels(); }
+			 * 
+			 * return pimg;
+			 */
+
+			if (pimg == null) {
+				pimg = new PImage(bufImg.getWidth(), bufImg.getHeight(),
+						PConstants.ARGB);
+				//DataBufferInt dbi = new DataBufferInt(pimg.pixels,
+					//	pimg.pixels.length);
+				//wr = Raster.createWritableRaster(bufImg.getSampleModel(), dbi,
+						//new Point(0, 0));
+			}
+			//bufImg.copyData(wr);
+			bufImg.getRGB(0, 0, pimg.width, pimg.height, pimg.pixels, 0,
+					pimg.width);
+			pimg.updatePixels();
+			return pimg;
+
+			/*
+			 * PImage img = new PImage(bufImg.getWidth(), bufImg.getHeight(),
+			 * PConstants.ARGB); bufImg.getRGB(0, 0, img.width, img.height,
+			 * img.pixels, 0, img.width); img.updatePixels(); return img;
+			 */
+
+		} catch (Exception e) {
+			// System.err.println("Can't create image from buffer");
+			// e.printStackTrace();
+		}
+		return null;
+	}
+
+	/**
+	 * resize bufferedimage
+	 * 
+	 * @param image
+	 * @param width
+	 * @param height
+	 * @return
+	 */
+	private BufferedImage resize(BufferedImage image, int width, int height) {
+		BufferedImage resizedImage = new BufferedImage(width, height,
+				BufferedImage.TYPE_INT_ARGB);
+		Graphics2D g = resizedImage.createGraphics();
+		g.drawImage(image, 0, 0, width, height, null);
+		g.dispose();
+		return resizedImage;
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/utils/ARDroneConstants.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.utils;
+
+public class ARDroneConstants {
+	
+	/** default IP address */
+	public static final String IP_ADDRESS = "192.168.1.1";
+	
+	/** default PORT */
+	public static final int PORT = 5556;
+	public static final int VIDEO_PORT = 5555;
+	public static final int NAV_PORT = 5554;
+	public static final int FTP_PORT = 5551;
+	
+	/** default ID, for AR.Drone 2.0 */
+	public static final String SESSION_ID = "d2e081a3"; 
+	public static final String PROFILE_ID = "be27e2e4";
+	public static final String APPLICATION_ID = "d87f7e0c";
+	
+	/** video codec */
+	public static final String VIDEO_CODEC_UVLC = "0x20"; // 320x240, 15fps for AR.Drone 1.0
+	public static final String VIDEO_CODEC_H264 = "0x40"; // 640x360, 20fps for AR.Drone 1.0
+	public static final String VIDEO_CODEC_360P = "0x81"; // 360p, for AR.Drone 2.0
+	public static final String VIDEO_CODEC_720P = "0x83"; // 720p, for AR.Drone 2.0
+
+}

src/com/shigeodayo/ardrone/utils/ARDroneInfo.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.utils;
+
+import java.io.BufferedOutputStream;
+import java.io.IOException;
+import java.io.OutputStream;
+
+import org.apache.commons.net.ftp.FTP;
+import org.apache.commons.net.ftp.FTPClient;
+
+import com.shigeodayo.ardrone.ARDrone;
+
+
+public class ARDroneInfo {
+
+	private static final String VERSION_FILE_NAME = "version.txt";
+	
+	private int major = -1;
+	private int minor = -1;
+	private int revision = -1;
+
+	private int count = 0;
+	
+	public ARDroneInfo() {
+		connectToDroneThroughFtp();
+	}
+
+	public ARDroneVersion getDroneVersion() {
+		//System.out.println("major:" + major);
+		switch (major) {
+		case 1:
+			return ARDroneVersion.ARDRONE1;
+		case 2:
+			return ARDroneVersion.ARDRONE2;
+		default:
+			return null;
+		}
+	}
+	
+	private boolean connectToDroneThroughFtp() {
+		FTPClient client = new FTPClient();
+		BufferedOutputStream bos = null;
+		
+        try {
+        	client.connect(ARDroneConstants.IP_ADDRESS, ARDroneConstants.FTP_PORT);
+
+        	if (!client.login("anonymous", "")) {
+        		ARDrone.error("Login failed", this);
+        		return false;
+        	}
+ 
+        	client.setFileType(FTP.BINARY_FILE_TYPE);
+
+        	bos = new BufferedOutputStream(new OutputStream() {
+				
+				@Override
+				public void write(int arg0) throws IOException {
+					//System.out.println("aa:" + (char)arg0);
+					switch (count) {
+					case 0:
+						major = arg0 - '0';
+						break;
+					case 2:
+						minor = arg0 - '0';
+						break;
+					case 4:
+						revision = arg0 - '0';
+						break;
+					default:
+						break;
+					}
+					count++;
+				}
+			});
+        	
+        	
+        	if (!client.retrieveFile("/" + VERSION_FILE_NAME, bos)) {
+            	ARDrone.error("Cannot find \"" + VERSION_FILE_NAME + "\"", this);
+            	return false;
+            }
+            
+            bos.flush();
+        	
+			//System.out.print("major:" + major);
+			//System.out.print(" minor:" + minor);
+			//System.out.println(" revision:" + revision);
+            
+            //System.out.println("done");
+        } catch (IOException e) {
+            e.printStackTrace();
+            return false;
+        } finally {
+            try {
+            	if (bos != null) {
+            		bos.flush();
+            		bos.close();
+            	}
+                client.disconnect();
+            } catch (IOException e) {
+                e.printStackTrace();
+                return false;
+            }
+        }
+        return true;
+	}
+}

src/com/shigeodayo/ardrone/utils/ARDroneVersion.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.utils;
+
+public enum ARDroneVersion {
+	ARDRONE1, ARDRONE2
+}

src/com/shigeodayo/ardrone/video/BufferedVideoImage.java

+package com.shigeodayo.ardrone.video;
+
+import java.nio.ByteBuffer;
+
+//#region Copyright Notice
+
+//Copyright  2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+////#endregion
+
+////#region Imports
+
+//using System;
+//using System.Collections.Generic;
+//using System.Text;
+////using System.Drawing;
+//using System.Runtime.InteropServices;
+//using System.Diagnostics;
+//using System.IO;
+//using System.Collections;
+//using System.Threading;
+
+//using System.Windows.Media;
+//using System.Windows.Media;
+//using System.Windows.Media.Imaging;
+
+//using Wilke.Interactive.Drone.Control.Enumerations;
+
+///#//#endregion
+
+public class BufferedVideoImage {
+	// event EventHandler<ImageCompleteEventArgs> ImageComplete;
+
+	// [DllImport("Kernel32.dll", EntryPoint = "RtlMoveMemory")]
+	// static extern void CopyMemory(IntPtr destination, IntPtr source, int
+	// length);
+
+	// /#//#region ants
+
+	private int _BlockWidth = 8;
+
+	private int _WidthCif = 88;
+	private int _HeightCif = 72;
+
+	private int _WidthVga = 160;
+	private int _HeightVga = 120;
+
+	private int _TableQuantization = 31;
+
+	private int FIX_0_298631336 = 2446;
+	private int FIX_0_390180644 = 3196;
+	private int FIX_0_541196100 = 4433;
+	private int FIX_0_765366865 = 6270;
+	private int FIX_0_899976223 = 7373;
+	private int FIX_1_175875602 = 9633;
+	private int FIX_1_501321110 = 12299;
+	private int FIX_1_847759065 = 15137;
+	private int FIX_1_961570560 = 16069;
+	private int FIX_2_053119869 = 16819;
+	private int FIX_2_562915447 = 20995;
+	private int FIX_3_072711026 = 25172;
+
+	private int _BITS = 13;
+	private int PASS1_BITS = 1;
+	private int F1 = _BITS - PASS1_BITS - 1;
+	private int F2 = _BITS - PASS1_BITS;
+	private int F3 = _BITS + PASS1_BITS + 3;
+
+	// /#//#endregion
+
+	// /#//#region Private Fields
+
+	private short[] dataBlockBuffer = new short[64];
+
+	private short[] zigZagPositions = new short[] { 0, 1, 8, 16, 9, 2, 3, 10,
+			17, 24, 32, 25, 18, 11, 4, 5, 12, 19, 26, 33, 40, 48, 41, 34, 27,
+			20, 13, 6, 7, 14, 21, 28, 35, 42, 49, 56, 57, 50, 43, 36, 29, 22,
+			15, 23, 30, 37, 44, 51, 58, 59, 52, 45, 38, 31, 39, 46, 53, 60, 61,
+			54, 47, 55, 62, 63, };
+
+	// Cfr. Handbook of Data Compression - Page 529
+	// David Salomon
+	// Giovanni Motta
+
+	private short[] quantizerValues = new short[] { 3, 5, 7, 9, 11, 13, 15, 17,
+			5, 7, 9, 11, 13, 15, 17, 19, 7, 9, 11, 13, 15, 17, 19, 21, 9, 11,
+			13, 15, 17, 19, 21, 23, 11, 13, 15, 17, 19, 21, 23, 25, 13, 15, 17,
+			19, 21, 23, 25, 27, 15, 17, 19, 21, 23, 25, 27, 29, 17, 19, 21, 23,
+			25, 27, 29, 31 };
+
+	static byte[] clzlut = new byte[] { 8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4,
+			4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2,
+			2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+			2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	// /#//#endregion
+
+	// /#//#region Private Properties
+
+	private uint StreamField;
+	private int StreamFieldBitIndex;
+	private int StreamIndex;
+	private int SliceCount;
+
+	public int getSliceCount() {
+		return SliceCount;
+	}
+
+	private boolean PictureComplete;
+
+	private int PictureFormat;
+	private int Resolution;
+	private int PictureType;
+
+	public int getPictureType() {
+		return PictureType;
+	}
+
+	private int QuantizerMode;
+	private int FrameIndex;
+
+	public int getFrameIndex() {
+		return FrameIndex;
+	}
+
+	private int SliceIndex;
+
+	private int BlockCount;
+
+	private int Width;
+	private int Height;
+
+	// / <summary>
+	// / Length of one row of pixels in the destination image in bytes.
+	// / </summary>
+	private int PixelRowSize;
+
+	public int getPixelRowSize() {
+		return PixelRowSize;
+	}
+
+	private ByteBuffer ImageStream;
+
+	private ImageSlice ImageSlice;
+	private uint[] PixelData;
+
+	public uint[] getPixelData() {
+		return PixelData;
+	}
+
+	// private WriteableBitmap ImageSource
+
+	// /#//#endregion
+
+	// /#//#region Properties
+
+	// WriteableBitmap ImageSource
+	// {
+	// get { return (WriteableBitmap)ImageSource.GetAsFrozen(); }
+	// }
+
+	// /#//#endregion
+
+	// /#//#region ruction
+
+	public BufferedVideoImage() {
+	}
+
+	// /#//#endregion
+
+	// /#//#region Methods
+
+	public void AddImageStream(ByteBuffer stream) {
+		ImageStream = stream;
+		ProcessStream();
+	}
+
+	// /#//#endregion
+
+	// /#//#region Private Methods
+
+	private void ProcessStream() {
+		boolean blockY0HasAcComponents = false;
+		boolean blockY1HasAcComponents = false;
+		boolean blockY2HasAcComponents = false;
+		boolean blockY3HasAcComponents = false;
+		boolean blockCbHasAcComponents = false;
+		boolean blockCrHasAcComponents = false;
+
+		// Set StreamFieldBitIndex to 32 to make sure that the first call to
+		// ReadStreamData
+		// actually consumes data from the stream
+		StreamFieldBitIndex = 32;
+		StreamField = new uint(0);
+		StreamIndex = 0;
+		SliceIndex = 0;
+		PictureComplete = false;
+
+		// Stopwatch stopWatch = new Stopwatch();
+		// stopWatch.Start();
+
+		while (!PictureComplete && StreamIndex < (ImageStream.capacity() >> 2)) {
+			ReadHeader();
+
+			if (!PictureComplete) {
+				for (int count = 0; count < BlockCount; count++) {
+					uint macroBlockEmpty = ReadStreamData(1);
+
+					if (macroBlockEmpty.intValue() == (0)) {
+						uint acCoefficients = ReadStreamData(8);
+
+						blockY0HasAcComponents = acCoefficients.shiftRight(0)
+								.and(1).intValue() == 1;
+						blockY1HasAcComponents = acCoefficients.shiftRight(1)
+								.and(1).intValue() == 1;
+						blockY2HasAcComponents = acCoefficients.shiftRight(2)
+								.and(1).intValue() == 1;
+						blockY3HasAcComponents = acCoefficients.shiftRight(3)
+								.and(1).intValue() == 1;
+						blockCbHasAcComponents = acCoefficients.shiftRight(4)
+								.and(1).intValue() == 1;
+						blockCrHasAcComponents = acCoefficients.shiftRight(5)
+								.and(1).intValue() == 1;
+
+						if (acCoefficients.shiftRight(6).and(1).intValue() == 1) {
+							uint quantizerMode = ReadStreamData(2);
+							QuantizerMode = (int) ((quantizerMode.intValue() < 2) ? quantizerMode
+									.flipBits() : quantizerMode.intValue());
+						}
+
+						// /#//#region Block Y0
+
+						GetBlockBytes(blockY0HasAcComponents);
+						InverseTransform(count, 0);
+
+						// /#//#endregion
+
+						// /#//#region Block Y1
+
+						GetBlockBytes(blockY1HasAcComponents);
+						InverseTransform(count, 1);
+
+						// /#//#endregion
+
+						// /#//#region Block Y2
+
+						GetBlockBytes(blockY2HasAcComponents);
+						InverseTransform(count, 2);
+
+						// /#//#endregion
+
+						// /#//#region Block Y3
+
+						GetBlockBytes(blockY3HasAcComponents);
+						InverseTransform(count, 3);
+
+						// /#//#endregion
+
+						// /#//#region Block Cb
+
+						GetBlockBytes(blockCbHasAcComponents);
+						InverseTransform(count, 4);
+
+						// /#//#endregion
+
+						// /#//#region Block Cr
+
+						GetBlockBytes(blockCrHasAcComponents);
+						InverseTransform(count, 5);
+
+						// /#//#endregion
+					}
+				}
+
+				ComposeImageSlice();
+			}
+		}
+
+		// System.out.println("PixelData Length " + PixelData.length);
+		// System.out.println("PixelRowSize " + PixelRowSize);
+		// System.out.println("Width " + Width);
+		// System.out.println("Height " + Height);
+		// System.out.println("Length/PixelRowSize "
+		// + (PixelData.length / PixelRowSize));
+
+		/*
+		 * unsafe { fixed (ushort* pixelData = PixelData) { IntPtr pixelDataPtr
+		 * = (IntPtr)pixelData; ImageSource.Lock();
+		 * CopyMemory(ImageSource.BackBuffer, pixelDataPtr, PixelData.Length *
+		 * 2); ImageSource.AddDirtyRect(Rectangle); ImageSource.Unlock(); } }
+		 * 
+		 * if (ImageComplete != null) { ImageComplete(this, new
+		 * ImageCompleteEventArgs(ImageSource)); }
+		 */
+	}
+
+	private void ReadHeader() {
+		uint code = new uint(0);
+		uint startCode = new uint(0);
+
+		AlignStreamData();
+
+		code = ReadStreamData(22);
+
+		startCode = new uint(code.and(~0x1F));
+
+		if (startCode.intValue() == 32) {
+			if (((code.and(0x1F).intValue()) == 0x1F)) {
+				PictureComplete = true;
+			} else {
+				if (SliceIndex++ == 0) {
+					PictureFormat = (int) ReadStreamData(2).intValue();
+					Resolution = (int) ReadStreamData(3).intValue();
+					PictureType = (int) ReadStreamData(3).intValue();
+					QuantizerMode = (int) ReadStreamData(5).intValue();
+					FrameIndex = (int) ReadStreamData(32).intValue();
+
+					switch (PictureFormat) {
+					case (int) PictureFormats.Cif:
+						Width = _WidthCif << Resolution - 1;
+						Height = _HeightCif << Resolution - 1;
+						break;
+					case (int) PictureFormats.Vga:
+						Width = _WidthVga << Resolution - 1;
+						Height = _HeightVga << Resolution - 1;
+						break;
+					}
+
+					// We assume two bytes per pixel (RGB 565)
+					PixelRowSize = Width << 1;
+
+					SliceCount = Height >> 4;
+					BlockCount = Width >> 4;
+
+					if (ImageSlice == null) {
+						ImageSlice = new ImageSlice(BlockCount);
+						PixelData = new uint[Width * Height];
+						// ImageSource = new WriteableBitmap(Width, Height, 96,
+						// 96, PixelFormats.Bgr565, null);
+						// Rectangle = new Int32Rect(0, 0, Width, Height);
+					} else {
+						if (ImageSlice.MacroBlocks.length != BlockCount) {
+							ImageSlice = new ImageSlice(BlockCount);
+							PixelData = new uint[Width * Height];
+							// ImageSource = new WriteableBitmap(Width, Height,
+							// 96, 96, PixelFormats.Bgr565, null);
+							// Rectangle = new Int32Rect(0, 0, Width, Height);
+						}
+					}
+				} else {
+					QuantizerMode = (int) ReadStreamData(5).intValue();
+				}
+			}
+		}
+	}
+
+	private void GetBlockBytes(boolean acCoefficientsAvailable) {
+		int[] run = new int[] { 0 };
+		int[] level = new int[] { 0 };
+		int zigZagPosition = 0;
+		int matrixPosition = 0;
+		boolean[] last = new boolean[] { false };
+
+		for (int i = 0; i < dataBlockBuffer.length; i++) {
+			dataBlockBuffer[i] = 0;
+		}
+		// Array.Clear(dataBlockBuffer, 0, dataBlockBuffer.length);
+
+		uint dcCoefficient = ReadStreamData(10);
+
+		if (QuantizerMode == _TableQuantization) {
+			dataBlockBuffer[0] = (short) (dcCoefficient
+					.times(quantizerValues[0]));
+
+			if (acCoefficientsAvailable) {
+				DecodeFieldBytes(run, level, last);
+
+				while (!last[0]) {
+					zigZagPosition += run[0] + 1;
+					matrixPosition = zigZagPositions[zigZagPosition];
+					level[0] *= quantizerValues[matrixPosition];
+					dataBlockBuffer[matrixPosition] = (short) level[0];
+					DecodeFieldBytes(run, level, last);
+				}
+			}
+		} else {
+			// Currently not implemented.
+			throw new RuntimeException(
+					"ant quantizer mode is not yet implemented.");
+		}
+	}
+
+	private void DecodeFieldBytes(int[] run, int[] level, boolean[] last) {
+		uint streamCode = new uint(0);
+
+		int streamLength = 0;
+		;
+		int zeroCount = 0;
+		int temp = 0;
+		int sign = 0;
+
+		// Use the RLE and Huffman dictionaries to understand this code
+		// fragment. You can find
+		// them in the developers guide on page 34.
+		// The bits in the data are actually composed of two kinds of fields:
+		// - run fields - this field contains information on the number of
+		// consecutive zeros.
+		// - level fields - this field contains the actual non zero value which
+		// can be negative or positive.
+		// First we extract the run field info and then the level field info.
+
+		streamCode = PeekStreamData(ImageStream, 32);
+
+		// /#//#region Determine number of consecutive zeros in zig zag. (a.k.a
+		// 'run' field info)
+
+		// Suppose we have following bit sequence:
+		// 00001111.....
+		// 1 - Count the number of leading zeros -> 4
+		// Coarse value lookup is thus 00001
+		// 2 - Lookup the additional value, for coarse value 00001 this is 3
+		// addtional bits
+		// 3 - Calculate value of run, for coarse value 00001 this is (111) + 8
+
+		zeroCount = CountLeadingZeros(streamCode); // - (1)
+		streamCode.shiftLeftEquals(zeroCount + 1); // - (2) -> shift left to get
+		// rid of the coarse value
+		streamLength += zeroCount + 1; // - position bit pointer to keep track
+		// off how many bits to consume later on
+		// the stream.
+
+		if (zeroCount > 1) {
+			temp = (streamCode.shiftRight(32 - (zeroCount - 1))).intValue(); // -
+			// (2)
+			// ->
+			// shift
+			// right
+			// to
+			// determine
+			// the
+			// addtional
+			// bits
+			// (number
+			// of
+			// additional
+			// bits
+			// is
+			// zerocount
+			// - 1)
+			streamCode.shiftLeftEquals(zeroCount - 1); // - shift all of the run
+			// bits out of the way
+			// so the first bit is
+			// points to the first
+			// bit of the level
+			// field.
+			streamLength += zeroCount - 1;// - position bit pointer to keep
+			// track off how many bits to
+			// consume later on the stream.
+			run[0] = temp + (1 << (zeroCount - 1)); // - (3) -> calculate run
+			// value
+		} else {
+			run[0] = zeroCount;
+		}
+
+		// /#//#endregion
+
+		// /#//#region Determine non zero value. (a.k.a 'level' field info)
+
+		// Suppose we have following bit sequence:
+		// 000011111.....
+		// 1 - Count the number of leading zeros -> 4
+		// Coarse value lookup is thus 00001
+		// 2 - Lookup the additional value, for coarse value 00001 this is 4
+		// addtional bits (last bit is sign bit)
+		// 3 - Calculate value of run, for coarse value 00001 this is (xxx) + 8,
+		// multiply by sign
+
+		zeroCount = CountLeadingZeros(streamCode);
+		streamCode.shiftLeftEquals(zeroCount + 1); // - (1)
+		streamLength += zeroCount + 1; // - position bit pointer to keep track
+		// off how many bits to consume later on
+		// the stream.
+
+		if (zeroCount == 1) {
+			// If coarse value is 01 according to the Huffman dictionary this
+			// means EOB, so there is
+			// no run and level and we indicate this by setting last to true;
+			run[0] = 0;
+			last[0] = true;
+		} else {
+			if (zeroCount == 0) {
+				zeroCount = 1;
+				temp = 1;
+			}
+
+			streamLength += zeroCount;// - position bit pointer to keep track
+			// off how many bits to consume later on
+			// the stream.
+			streamCode.shiftRightEquals(32 - zeroCount);// - (2) -> shift right
+			// to determine the
+			// addtional bits
+			// (number of additional
+			// bits is zerocount)
+			// sign = (sbyte)(streamCode & 1); // determine sign, last bit is
+			// sign
+			sign = (int) (streamCode.and(1).intValue()); // determine sign, last
+			// bit is sign
+
+			if (zeroCount != 0) {
+				// temp = (sbyte)(streamCode >> 1); // take into account that
+				// last bit is sign, so shift it out of the way
+				// temp += (sbyte)(1 << (zeroCount - 1)); // - (3) -> calculate
+				// run value without sign
+				temp = (streamCode.shiftRight(1)).intValue(); // take into
+				// account
+				// that last bit is
+				// sign, so shift it
+				// out of the way
+				temp += (int) (1 << (zeroCount - 1)); // - (3) -> calculate run
+				// value without sign
+			}
+
+			level[0] = (sign == 1) ? -temp : temp; // - (3) -> calculate run
+			// value with sign
+			last[0] = false;
+		}
+
+		// /#//#endregion
+
+		ReadStreamData(streamLength);
+	}
+
+	int numCalls = 0;
+
+	private uint ReadStreamData(int count) {
+		uint data = new uint(0);
+
+		while (count > (32 - StreamFieldBitIndex)) {
+			data = (data.shiftLeft((int) (32 - StreamFieldBitIndex))
+					.or(StreamField.shiftRight(StreamFieldBitIndex)));
+
+			count -= 32 - StreamFieldBitIndex;
+
+			StreamField = new uint(ImageStream, StreamIndex * 4);
+
+			StreamFieldBitIndex = 0;
+			StreamIndex++;
+		}
+
+		if (count > 0) {
+			data = data.shiftLeft(count).or(StreamField.shiftRight(32 - count));
+
+			StreamField.shiftLeftEquals(count);
+			StreamFieldBitIndex += count;
+		}
+
+		numCalls++;
+		// System.out.println("ReadStreamData " + data + " " + numCalls + " " +
+		// count);
+
+		return data;
+	}
+
+	private uint PeekStreamData(ByteBuffer stream, int count) {
+		uint data = new uint(0);
+		uint streamField = StreamField;
+		int streamFieldBitIndex = StreamFieldBitIndex;
+
+		while (count > (32 - streamFieldBitIndex)
+				&& StreamIndex < (ImageStream.capacity() >> 2)) {
+			data = (data.shiftLeft(32 - streamFieldBitIndex)).or(streamField
+					.shiftRight(streamFieldBitIndex));
+
+			count -= 32 - streamFieldBitIndex;
+
+			streamField = new uint(stream, StreamIndex * 4);
+			streamFieldBitIndex = 0;
+		}
+
+		if (count > 0) {
+			data = data.shiftLeft(count).or(
+					streamField.shiftRight((32 - count)));
+		}
+
+		return data;
+	}
+
+	private void AlignStreamData() {
+		int alignedLength;
+		int actualLength;
+
+		actualLength = StreamFieldBitIndex;
+
+		if (actualLength > 0) {
+			alignedLength = (actualLength & ~7);
+			if (alignedLength != actualLength) {
+				alignedLength += 0x08;
+				StreamField.shiftLeftEquals(alignedLength - actualLength);
+				StreamFieldBitIndex = alignedLength;
+			}
+		}
+	}
+
+	// Blockline:
+	// _______
+	// | 1 | 2 |
+	// |___|___| Y
+	// | 3 | 4 |
+	// |___|___|
+	// ___
+	// | 5 |
+	// |___| Cb
+	// ___
+	// | 6 |
+	// |___| Cr
+	//
+	// Layout in memory
+	// _______________________
+	// | 1 | 2 | 3 | 4 | 5 | 6 | ...
+	// |___|___|___|___|___|___|
+	//
+
+	// Example, suppose the six data sub blocks are as follows:
+
+	// ==============Y0============== ==============Y1==============
+	// ==============Y2============== ==============Y3==============
+
+	// 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7,
+	// 0, 1, 2, 3, 4, 5, 6, 7,
+	// 8, 9, 10, 11, 12, 13, 14, 15, 8, 9, 10, 11, 12, 13, 14, 15, 8, 9, 10, 11,
+	// 12, 13, 14, 15, 8, 9, 10, 11, 12, 13, 14, 15,
+	// 16, 17, 18, 19, 20, 21, 22, 23, 16, 17, 18, 19, 20, 21, 22, 23, 16, 17,
+	// 18, 19, 20, 21, 22, 23, 16, 17, 18, 19, 20, 21, 22, 23,
+	// 24, 25, 26, 27, 28, 29, 30, 31, 24, 25, 26, 27, 28, 29, 30, 31, 24, 25,
+	// 26, 27, 28, 29, 30, 31, 24, 25, 26, 27, 28, 29, 30, 31,
+	// 32, 33, 34, 35, 36, 37, 38, 39, 32, 33, 34, 35, 36, 37, 38, 39, 32, 33,
+	// 34, 35, 36, 37, 38, 39, 32, 33, 34, 35, 36, 37, 38, 39,
+	// 40, 41, 42, 43, 44, 45, 46, 47, 40, 41, 42, 43, 44, 45, 46, 47, 40, 41,
+	// 42, 43, 44, 45, 46, 47, 40, 41, 42, 43, 44, 45, 46, 47,
+	// 48, 49, 50, 51, 52, 53, 54, 55, 48, 49, 50, 51, 52, 53, 54, 55, 48, 49,
+	// 50, 51, 52, 53, 54, 55, 48, 49, 50, 51, 52, 53, 54, 55,
+	// 56, 57, 58, 59, 60, 61, 62, 63, 56, 57, 58, 59, 60, 61, 62, 63, 56, 57,
+	// 58, 59, 60, 61, 62, 63, 56, 57, 58, 59, 60, 61, 62, 63
+
+	// ==============Cb============== ==============Cr==============
+
+	// 0, 1, 2, 3, | 4, 5, 6, 7, 0, 1, 2, 3, | 4, 5, 6, 7,
+	// 8, 9, 10, 11, | 12, 13, 14, 15, 8, 9, 10, 11, | 12, 13, 14, 15,
+	// 16, 17, 18, 19, | 20, 21, 22, 23, 16, 17, 18, 19, | 20, 21, 22, 23,
+	// 24, 25, 26, 27, | 28, 29, 30, 31, 24, 25, 26, 27, | 28, 29, 30, 31,
+	// ----------------| --------------- --------------- | ---------------
+	// 32, 33, 34, 35, | 36, 37, 38, 39, 32, 33, 34, 35, | 36, 37, 38, 39,
+	// 40, 41, 42, 43, | 44, 45, 46, 47, 40, 41, 42, 43, | 44, 45, 46, 47,
+	// 48, 49, 50, 51, | 52, 53, 54, 55, 48, 49, 50, 51, | 52, 53, 54, 55,
+	// 56, 57, 58, 59, | 60, 61, 62, 63, 56, 57, 58, 59, | 60, 61, 62, 63,
+
+	// Pixel Matrix
+
+	// 0, 1, 2, 3, 4, 5, 6, 7, | 8, 9, 10, 11, 12, 13, 14, 15,
+	// 16, 17, 18, 19, 20, 21, 22, 23, | 24, 25, 26, 27, 28, 29, 30, 31,
+	// 32, 33, 34, 35, 36, 37, 38, 39, | 40, 41, 42, 43, 44, 45, 46, 47,
+	// 48, 49, 50, 51, 52, 53, 54, 55, | 56, 57, 58, 59, 60, 61, 62, 63,
+	// 64, 65, 66, 67, 68, 69, 70, 71, | 72, 73, 74, 75, 76, 77, 78, 79,
+	// 80, 81, 82, 83, 84, 85, 86, 87, | 88, 89, 90, 91, 92, 93, 94, 95,
+	// 96, 97, 98, 99, 100, 101, 102, 103, | 104, 105, 106, 107, 108, 109, 110,
+	// 111,
+	// 112, 113, 114, 115, 116, 117, 118, 119, | 120, 121, 122, 123, 124, 125,
+	// 126, 127,
+	// ----------------------------------------|---------------------------------------
+	// 128, 129, 130, 131, 132, 133, 134, 135, | 136, 137, 138, 139, 140, 141,
+	// 142, 143,
+	// 144, 145, 146, 147, 148, 149, 150, 151, | 152, 153, 154, 155, 156, 157,
+	// 158, 159,
+	// 160, 161, 162, 163, 164, 165, 166, 167, | 168, 169, 170, 171, 172, 173,
+	// 174, 175,
+	// 176, 177, 178, 179, 180, 181, 182, 183, | 184, 185, 186, 187, 188, 189,
+	// 190, 191,
+	// 192, 193, 194, 195, 196, 197, 198, 199, | 200, 201, 202, 203, 204, 205,
+	// 206, 207,
+	// 208, 209, 210, 211, 212, 213, 214, 215, | 216, 217, 218, 219, 220, 221,
+	// 222, 223,
+	// 224, 225, 226, 227, 228, 229, 230, 231, | 232, 233, 234, 235, 236, 237,
+	// 238, 239,
+	// 240, 241, 242, 243, 244, 245, 246, 247, | 248, 249, 250, 251, 252, 253,
+	// 254, 255,
+
+	// The four Luma 8x8 matrices (quadrants Y0, Y1, Y2, Y3) form the basis of
+	// the final 16x16 pixel matrix.
+	// The two Croma 8x8 matrices are used to calculate the actual RGB value of
+	// the pixel (RGB565, each pixel is represented by two bytes)
+
+	// Each processing loop processes from each Luma matrix two rows. In each
+	// 'two row' loop the rows are processed
+	// by two columns.
+
+	// First Loop will take (assume there is only one pixel matrix to fill):
+
+	// Quadrant 1
+	// From Cb -> 0
+	// From Cr -> 0
+	// From Y0 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 0, 16 and 1 and 17
+
+	// Quadrant 2
+	// From Cb -> 4
+	// From Cr -> 4
+	// From Y1 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 8, 24 and 9 and 25
+
+	// Quadrant 3
+	// From Cb -> 32
+	// From Cr -> 32
+	// From Y2 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 128, 144 and 129 and 145
+
+	// Quadrant 4
+	// From Cb -> 36
+	// From Cr -> 36
+	// From Y3 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 136, 152 and 137 and 153
+
+	// Second Loop will take (assume there is only one pixel matrix to fill):
+
+	// Quadrant 1
+	// From Cb -> 1
+	// From Cr -> 1
+	// From Y0 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 2, 18 and 3 and 19
+
+	// Quadrant 2
+	// From Cb -> 5
+	// From Cr -> 5
+	// From Y1 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 10, 26 and 11 and 27
+
+	// Quadrant 3
+	// From Cb -> 33
+	// From Cr -> 33
+	// From Y2 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 130, 146 and 131 and 147
+	// Quadrant 4
+	// From Cb -> 37
+	// From Cr -> 37
+	// From Y3 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 138, 154 and 139 and 155
+
+	// We need third and fourth loop to complete first two lines of the luma
+	// blocks. At this time we
+	// have written 64 pixels to the pixel matrix.
+
+	// These four loops have to be repeated 4 more times (4 * 64 = 256) to fill
+	// complete pixel matrix.
+
+	// Remark the offsets to use in the pixel matrix have to take into account
+	// that an GroupOfBlocks contains multiple pixel matrices.
+	// So to calculate the real index we have to take that also into account
+	// (BlockCount)
+
+	private void ComposeImageSlice() {
+		int u, ug, ub;
+		int v, vg, vr;
+		int r, g, b;
+
+		int lumaElementIndex1 = 0;
+		int lumaElementIndex2 = 0;
+		int chromaOffset = 0;
+
+		int dataIndex1 = 0;
+		int dataIndex2 = 0;
+
+		int lumaElementValue1 = 0;
+		int lumaElementValue2 = 0;
+		int chromaBlueValue = 0;
+		int chromaRedValue = 0;
+
+		int[] cromaQuadrantOffsets = new int[] { 0, 4, 32, 36 };
+		int[] pixelDataQuadrantOffsets = new int[] { 0, _BlockWidth,
+				Width * _BlockWidth, (Width * _BlockWidth) + _BlockWidth };
+
+		int imageDataOffset = (SliceIndex - 1) * Width * 16;
+
+		for (MacroBlock macroBlock : ImageSlice.MacroBlocks) {
+			for (int verticalStep = 0; verticalStep < _BlockWidth / 2; verticalStep++) {
+				chromaOffset = verticalStep * _BlockWidth;
+				lumaElementIndex1 = verticalStep * _BlockWidth * 2;
+				lumaElementIndex2 = lumaElementIndex1 + _BlockWidth;
+
+				dataIndex1 = imageDataOffset + (2 * verticalStep * Width);
+				dataIndex2 = dataIndex1 + Width;
+
+				for (int horizontalStep = 0; horizontalStep < _BlockWidth / 2; horizontalStep++) {
+					for (int quadrant = 0; quadrant < 4; quadrant++) {
+						int chromaIndex = chromaOffset
+								+ cromaQuadrantOffsets[quadrant]
+								+ horizontalStep;
+						chromaBlueValue = macroBlock.DataBlocks[4][chromaIndex];
+						chromaRedValue = macroBlock.DataBlocks[5][chromaIndex];
+
+						u = chromaBlueValue - 128;
+						ug = 88 * u;
+						ub = 454 * u;
+
+						v = chromaRedValue - 128;
+						vg = 183 * v;
+						vr = 359 * v;
+
+						for (int pixel = 0; pixel < 2; pixel++) {
+							int deltaIndex = 2 * horizontalStep + pixel;
+							lumaElementValue1 = macroBlock.DataBlocks[quadrant][lumaElementIndex1
+									+ deltaIndex] << 8;
+							lumaElementValue2 = macroBlock.DataBlocks[quadrant][lumaElementIndex2
+									+ deltaIndex] << 8;
+
+							r = Saturate5(lumaElementValue1 + vr);
+							g = Saturate6(lumaElementValue1 - ug - vg);
+							b = Saturate5(lumaElementValue1 + ub);
+
+							PixelData[dataIndex1
+									+ pixelDataQuadrantOffsets[quadrant]
+									+ deltaIndex] = MakeRgb(r, g, b);
+
+							r = Saturate5(lumaElementValue2 + vr);
+							g = Saturate6(lumaElementValue2 - ug - vg);
+							b = Saturate5(lumaElementValue2 + ub);
+
+							PixelData[dataIndex2
+									+ pixelDataQuadrantOffsets[quadrant]
+									+ deltaIndex] = MakeRgb(r, g, b);
+						}
+					}
+				}
+			}
+
+			imageDataOffset += 16;
+		}
+	}
+
+	private int Saturate5(int x) {
+		if (x < 0) {
+			x = 0;
+		}
+
+		x >>= 11;
+
+		return (x > 0x1F) ? 0x1F : x;
+	}
+
+	private int Saturate6(int x) {
+		if (x < 0) {
+			x = 0;
+		}
+
+		x >>= 10;
+
+		return x > 0x3F ? 0x3F : x;
+	}
+
+	private uint MakeRgb(int r, int g, int b) {
+		r <<= 2;
+		g <<= 1;
+		b <<= 2;
+
+		uint ru = new uint(r);
+		uint gu = new uint(g);
+		uint bu = new uint(b);
+
+		uint retval = ru.shiftLeft(16);
+		retval = retval.or(gu.shiftLeft(8));
+		retval = retval.or(bu);
+
+		return retval;
+		// return new newUint((r << 16) | (g << 8) | b);
+	}
+
+	private int CountLeadingZeros(uint value) {
+		int accum = 0;
+
+		accum += clzlut[value.shiftRight(24).intValue()];
+		if (accum == 8) {
+			accum += clzlut[(value.shiftRight(16).intValue()) & 0xFF];
+		}
+		if (accum == 16) {
+			accum += clzlut[(value.shiftRight(8).intValue()) & 0xFF];
+		}
+		if (accum == 24) {
+			accum += clzlut[value.intValue() & 0xFF];
+		}
+
+		return accum;
+	}
+
+	// /#//#region Dct Methods
+
+	void InverseTransform(int macroBlockIndex, int dataBlockIndex) {
+		int[] workSpace = new int[64];
+		short[] data = new short[64];
+
+		int z1, z2, z3, z4, z5;
+		int tmp0, tmp1, tmp2, tmp3;
+		int tmp10, tmp11, tmp12, tmp13;
+
+		int pointer = 0;
+
+		for (int index = 8; index > 0; index--) {
+			if (dataBlockBuffer[pointer + 8] == 0
+					&& dataBlockBuffer[pointer + 16] == 0
+					&& dataBlockBuffer[pointer + 24] == 0
+					&& dataBlockBuffer[pointer + 32] == 0
+					&& dataBlockBuffer[pointer + 40] == 0
+					&& dataBlockBuffer[pointer + 48] == 0
+					&& dataBlockBuffer[pointer + 56] == 0) {
+				int dcValue = dataBlockBuffer[pointer] << PASS1_BITS;
+
+				workSpace[pointer + 0] = dcValue;
+				workSpace[pointer + 8] = dcValue;
+				workSpace[pointer + 16] = dcValue;
+				workSpace[pointer + 24] = dcValue;
+				workSpace[pointer + 32] = dcValue;
+				workSpace[pointer + 40] = dcValue;
+				workSpace[pointer + 48] = dcValue;
+				workSpace[pointer + 56] = dcValue;
+
+				pointer++;
+				continue;
+			}
+
+			z2 = dataBlockBuffer[pointer + 16];
+			z3 = dataBlockBuffer[pointer + 48];
+
+			z1 = (z2 + z3) * FIX_0_541196100;
+			tmp2 = z1 + z3 * -FIX_1_847759065;
+			tmp3 = z1 + z2 * FIX_0_765366865;
+
+			z2 = dataBlockBuffer[pointer];
+			z3 = dataBlockBuffer[pointer + 32];
+
+			tmp0 = (z2 + z3) << _BITS;
+			tmp1 = (z2 - z3) << _BITS;
+
+			tmp10 = tmp0 + tmp3;
+			tmp13 = tmp0 - tmp3;
+			tmp11 = tmp1 + tmp2;
+			tmp12 = tmp1 - tmp2;
+
+			tmp0 = dataBlockBuffer[pointer + 56];
+			tmp1 = dataBlockBuffer[pointer + 40];
+			tmp2 = dataBlockBuffer[pointer + 24];
+			tmp3 = dataBlockBuffer[pointer + 8];
+
+			z1 = tmp0 + tmp3;
+			z2 = tmp1 + tmp2;
+			z3 = tmp0 + tmp2;
+			z4 = tmp1 + tmp3;
+			z5 = (z3 + z4) * FIX_1_175875602;
+
+			tmp0 = tmp0 * FIX_0_298631336;
+			tmp1 = tmp1 * FIX_2_053119869;
+			tmp2 = tmp2 * FIX_3_072711026;
+			tmp3 = tmp3 * FIX_1_501321110;
+			z1 = z1 * -FIX_0_899976223;
+			z2 = z2 * -FIX_2_562915447;
+			z3 = z3 * -FIX_1_961570560;
+			z4 = z4 * -FIX_0_390180644;
+
+			z3 += z5;
+			z4 += z5;
+
+			tmp0 += z1 + z3;
+			tmp1 += z2 + z4;
+			tmp2 += z2 + z3;
+			tmp3 += z1 + z4;
+
+			workSpace[pointer + 0] = ((tmp10 + tmp3 + (1 << F1)) >> F2);
+			workSpace[pointer + 56] = ((tmp10 - tmp3 + (1 << F1)) >> F2);
+			workSpace[pointer + 8] = ((tmp11 + tmp2 + (1 << F1)) >> F2);
+			workSpace[pointer + 48] = ((tmp11 - tmp2 + (1 << F1)) >> F2);
+			workSpace[pointer + 16] = ((tmp12 + tmp1 + (1 << F1)) >> F2);
+			workSpace[pointer + 40] = ((tmp12 - tmp1 + (1 << F1)) >> F2);
+			workSpace[pointer + 24] = ((tmp13 + tmp0 + (1 << F1)) >> F2);
+			workSpace[pointer + 32] = ((tmp13 - tmp0 + (1 << F1)) >> F2);
+
+			pointer++;
+		}
+
+		pointer = 0;
+
+		for (int index = 0; index < 8; index++) {
+			z2 = workSpace[pointer + 2];
+			z3 = workSpace[pointer + 6];
+
+			z1 = (z2 + z3) * FIX_0_541196100;
+			tmp2 = z1 + z3 * -FIX_1_847759065;
+			tmp3 = z1 + z2 * FIX_0_765366865;
+
+			tmp0 = (workSpace[pointer + 0] + workSpace[pointer + 4]) << _BITS;
+			tmp1 = (workSpace[pointer + 0] - workSpace[pointer + 4]) << _BITS;
+
+			tmp10 = tmp0 + tmp3;
+			tmp13 = tmp0 - tmp3;
+			tmp11 = tmp1 + tmp2;
+			tmp12 = tmp1 - tmp2;
+
+			tmp0 = workSpace[pointer + 7];
+			tmp1 = workSpace[pointer + 5];
+			tmp2 = workSpace[pointer + 3];
+			tmp3 = workSpace[pointer + 1];
+
+			z1 = tmp0 + tmp3;
+			z2 = tmp1 + tmp2;
+			z3 = tmp0 + tmp2;
+			z4 = tmp1 + tmp3;
+
+			z5 = (z3 + z4) * FIX_1_175875602;
+
+			tmp0 = tmp0 * FIX_0_298631336;
+			tmp1 = tmp1 * FIX_2_053119869;
+			tmp2 = tmp2 * FIX_3_072711026;
+			tmp3 = tmp3 * FIX_1_501321110;
+			z1 = z1 * -FIX_0_899976223;
+			z2 = z2 * -FIX_2_562915447;
+			z3 = z3 * -FIX_1_961570560;
+			z4 = z4 * -FIX_0_390180644;
+
+			z3 += z5;
+			z4 += z5;
+
+			tmp0 += z1 + z3;
+			tmp1 += z2 + z4;
+			tmp2 += z2 + z3;
+			tmp3 += z1 + z4;
+
+			data[pointer + 0] = (short) ((tmp10 + tmp3) >> F3);
+			data[pointer + 7] = (short) ((tmp10 - tmp3) >> F3);
+			data[pointer + 1] = (short) ((tmp11 + tmp2) >> F3);
+			data[pointer + 6] = (short) ((tmp11 - tmp2) >> F3);
+			data[pointer + 2] = (short) ((tmp12 + tmp1) >> F3);
+			data[pointer + 5] = (short) ((tmp12 - tmp1) >> F3);
+			data[pointer + 3] = (short) ((tmp13 + tmp0) >> F3);
+			data[pointer + 4] = (short) ((tmp13 - tmp0) >> F3);
+
+			pointer += 8;
+		}
+
+		for (int i = 0; i < data.length; i++) {
+			ImageSlice.MacroBlocks[macroBlockIndex].DataBlocks[dataBlockIndex][i] = data[i];
+		}
+		/*
+		 * unsafe { fixed (short* source = data) fixed (short* destination =
+		 * ImageSlice.MacroBlocks[macroBlockIndex].DataBlocks[dataBlockIndex]) {
+		 * IntPtr sourcePtr = (IntPtr)source; IntPtr destinationPtr =
+		 * (IntPtr)destination; CopyMemory(destinationPtr, sourcePtr,
+		 * data.Length * 2); } }
+		 */
+	}
+}
+
+// /#//#endregion
+
+// /#//#endregion

src/com/shigeodayo/ardrone/video/ImageDataException.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.video;
+
+public class ImageDataException extends Exception {
+
+	private static final long serialVersionUID = -3948568506319692544L;
+
+	public ImageDataException(String message) {
+		super(message);
+	}
+}

src/com/shigeodayo/ardrone/video/ImageListener.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.video;
+
+import java.awt.image.BufferedImage;
+
+public interface ImageListener {
+	void imageUpdated(BufferedImage image);
+}

src/com/shigeodayo/ardrone/video/ImageSlice.java

+package com.shigeodayo.ardrone.video;
+
+//Copyright © 2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+public class ImageSlice {
+	MacroBlock[] MacroBlocks;
+
+	ImageSlice(int macroBlockCount) {
+		MacroBlocks = new MacroBlock[macroBlockCount];
+
+		for (int index = 0; index < macroBlockCount; index++) {
+			MacroBlocks[index] = new MacroBlock();
+		}
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/video/MacroBlock.java

+package com.shigeodayo.ardrone.video;
+
+//Copyright © 2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+public class MacroBlock {
+	// /#//#region ants
+
+	// private int _BlockWidth = 8;
+	// private int _BlockSize = 64;
+
+	// //#endregion
+
+	// //#region Properties
+
+	short[][] DataBlocks;
+
+	// //#endregion
+
+	// //#region ruction
+
+	MacroBlock() {
+		DataBlocks = new short[6][];
+
+		for (int index = 0; index < 6; index++) {
+			DataBlocks[index] = new short[64];
+		}
+	}
+
+	// //#endregion
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/video/PictureFormats.java

+package com.shigeodayo.ardrone.video;
+
+//Copyright © 2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+public class PictureFormats {
+	// / <summary>
+	// / 176px x 144px
+	// / </summary>
+	public static final int Cif = 1;
+	// / <summary>
+	// / 320px x 240px
+	// / </summary>
+	public static final int Vga = 2;
+}

src/com/shigeodayo/ardrone/video/ReadRawFileImage.java

+//Copyright  2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//  specific prior written permission.
+
+//modified :Shigeo Yoshida, 2011-02-23
+package com.shigeodayo.ardrone.video;
+
+import java.awt.image.BufferedImage;
+import java.io.FileNotFoundException;
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class ReadRawFileImage {
+
+	public ReadRawFileImage() {
+	}
+
+	public BufferedImage readUINT_RGBImage(byte[] rawData)
+			throws FileNotFoundException, IOException {
+		int length = 0;
+		try {
+			byte[] processedData = process(rawData);
+
+			int[] pixelData = new int[processedData.length / 3];
+			int raw, pixel = 0, j = 0;
+			for (int i = 0; i < pixelData.length; i++) {
+				pixel = 0;
+				raw = processedData[j++] & 0xFF;
+				pixel |= (raw << 16);
+				raw = processedData[j++] & 0xFF;
+				pixel |= (raw << 8);
+				raw = processedData[j++] & 0xFF;
+				pixel |= (raw << 0);
+
+				pixelData[i] = pixel;
+			}
+			// System.out.println(pixelData.length);
+			length = pixelData.length;
+
+			if (length == 76800) {
+				BufferedImage image = new BufferedImage(320, 240,
+						BufferedImage.TYPE_INT_RGB);
+
+				image.setRGB(0, 0, 320, 240, pixelData, 0, 320);
+				return image;
+			} else if (length == 25344) {
+				BufferedImage image = new BufferedImage(176, 144,
+						BufferedImage.TYPE_INT_RGB);
+
+				image.setRGB(0, 0, 176, 144, pixelData, 0, 176);
+				return image;
+			}
+			/*
+			 * BufferedImage image = new BufferedImage(640, 480,
+			 * BufferedImage.TYPE_INT_RGB);
+			 * 
+			 * image.setRGB(0, 0, 640, 480, pixelData, 0, 480);
+			 */
+
+		} catch (ArrayIndexOutOfBoundsException e) {
+			e.printStackTrace();
+			// System.out.println(length);
+		}
+		return null;
+	}
+
+	private byte[] process(final byte[] rawData) {
+		final BufferedVideoImage image = new BufferedVideoImage();
+		image.AddImageStream(ByteBuffer.wrap(rawData));
+
+		final uint[] outData = image.getPixelData();
+
+		ByteBuffer buffer = ByteBuffer.allocate(outData.length * 3);
+
+		for (int i = 0; i < outData.length; i++) {
+			int myInt = outData[i].intValue();
+			buffer.put((byte) ((myInt >> 16) & 0xFF));
+			buffer.put((byte) ((myInt >> 8) & 0xFF));
+			buffer.put((byte) (myInt & 0xFF));
+
+		}
+		return buffer.array();
+	}
+}

src/com/shigeodayo/ardrone/video/VideoImage.java

+package com.shigeodayo.ardrone.video;
+
+//#region Copyright Notice
+
+//Copyright  2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+////#endregion
+
+////#region Imports
+
+//using System;
+//using System.Collections.Generic;
+//using System.Text;
+////using System.Drawing;
+//using System.Runtime.InteropServices;
+//using System.Diagnostics;
+//using System.IO;
+//using System.Collections;
+//using System.Threading;
+
+//using System.Windows.Media;
+//using System.Windows.Media;
+//using System.Windows.Media.Imaging;
+
+//using Wilke.Interactive.Drone.Control.Enumerations;
+
+///#//#endregion
+
+public class VideoImage {
+	// event EventHandler<ImageCompleteEventArgs> ImageComplete;
+
+	// [DllImport("Kernel32.dll", EntryPoint = "RtlMoveMemory")]
+	// static extern void CopyMemory(IntPtr destination, IntPtr source, int
+	// length);
+
+	// /#//#region ants
+
+	private int _BlockWidth = 8;
+
+	private int _WidthCif = 88;
+	private int _HeightCif = 72;
+
+	private int _WidthVga = 160;
+	private int _HeightVga = 120;
+
+	private int _TableQuantization = 31;
+
+	private int FIX_0_298631336 = 2446;
+	private int FIX_0_390180644 = 3196;
+	private int FIX_0_541196100 = 4433;
+	private int FIX_0_765366865 = 6270;
+	private int FIX_0_899976223 = 7373;
+	private int FIX_1_175875602 = 9633;
+	private int FIX_1_501321110 = 12299;
+	private int FIX_1_847759065 = 15137;
+	private int FIX_1_961570560 = 16069;
+	private int FIX_2_053119869 = 16819;
+	private int FIX_2_562915447 = 20995;
+	private int FIX_3_072711026 = 25172;
+
+	private int _BITS = 13;
+	private int PASS1_BITS = 1;
+	private int F1 = _BITS - PASS1_BITS - 1;
+	private int F2 = _BITS - PASS1_BITS;
+	private int F3 = _BITS + PASS1_BITS + 3;
+
+	// /#//#endregion
+
+	// /#//#region Private Fields
+
+	private short[] dataBlockBuffer = new short[64];
+
+	private short[] zigZagPositions = new short[] { 0, 1, 8, 16, 9, 2, 3, 10,
+			17, 24, 32, 25, 18, 11, 4, 5, 12, 19, 26, 33, 40, 48, 41, 34, 27,
+			20, 13, 6, 7, 14, 21, 28, 35, 42, 49, 56, 57, 50, 43, 36, 29, 22,
+			15, 23, 30, 37, 44, 51, 58, 59, 52, 45, 38, 31, 39, 46, 53, 60, 61,
+			54, 47, 55, 62, 63, };
+
+	// Cfr. Handbook of Data Compression - Page 529
+	// David Salomon
+	// Giovanni Motta
+
+	private short[] quantizerValues = new short[] { 3, 5, 7, 9, 11, 13, 15, 17,
+			5, 7, 9, 11, 13, 15, 17, 19, 7, 9, 11, 13, 15, 17, 19, 21, 9, 11,
+			13, 15, 17, 19, 21, 23, 11, 13, 15, 17, 19, 21, 23, 25, 13, 15, 17,
+			19, 21, 23, 25, 27, 15, 17, 19, 21, 23, 25, 27, 29, 17, 19, 21, 23,
+			25, 27, 29, 31 };
+
+	static byte[] clzlut = new byte[] { 8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4,
+			4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2,
+			2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+			2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+			1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+	// /#//#endregion
+
+	// /#//#region Private Properties
+
+	private uint StreamField;
+	private int StreamFieldBitIndex;
+	private int StreamIndex;
+	private int SliceCount;
+
+	public int getSliceCount() {
+		return SliceCount;
+	}
+
+	private boolean PictureComplete;
+
+	private int PictureFormat;
+	private int Resolution;
+	private int PictureType;
+
+	public int getPictureType() {
+		return PictureType;
+	}
+
+	private int QuantizerMode;
+	private int FrameIndex;
+
+	public int getFrameIndex() {
+		return FrameIndex;
+	}
+
+	private int SliceIndex;
+
+	private int BlockCount;
+
+	private int Width;
+	private int Height;
+
+	// / <summary>
+	// / Length of one row of pixels in the destination image in bytes.
+	// / </summary>
+	private int PixelRowSize;
+
+	public int getPixelRowSize() {
+		return PixelRowSize;
+	}
+
+	private byte[] ImageStream;
+
+	private ImageSlice ImageSlice;
+	private uint[] PixelData;
+
+	public uint[] getPixelData() {
+		return PixelData;
+	}
+
+	// private WriteableBitmap ImageSource
+
+	// /#//#endregion
+
+	// /#//#region Properties
+
+	// WriteableBitmap ImageSource
+	// {
+	// get { return (WriteableBitmap)ImageSource.GetAsFrozen(); }
+	// }
+
+	// /#//#endregion
+
+	// /#//#region ruction
+
+	public VideoImage() {
+	}
+
+	// /#//#endregion
+
+	// /#//#region Methods
+
+	public void AddImageStream(byte[] stream) {
+		ImageStream = stream;
+		ProcessStream();
+	}
+
+	// /#//#endregion
+
+	// /#//#region Private Methods
+
+	private void ProcessStream() {
+		boolean blockY0HasAcComponents = false;
+		boolean blockY1HasAcComponents = false;
+		boolean blockY2HasAcComponents = false;
+		boolean blockY3HasAcComponents = false;
+		boolean blockCbHasAcComponents = false;
+		boolean blockCrHasAcComponents = false;
+
+		// Set StreamFieldBitIndex to 32 to make sure that the first call to
+		// ReadStreamData
+		// actually consumes data from the stream
+		StreamFieldBitIndex = 32;
+		StreamField = new uint(0);
+		StreamIndex = 0;
+		SliceIndex = 0;
+		PictureComplete = false;
+
+		// Stopwatch stopWatch = new Stopwatch();
+		// stopWatch.Start();
+
+		while (!PictureComplete && StreamIndex < (ImageStream.length >> 2)) {
+			ReadHeader();
+
+			if (!PictureComplete) {
+				for (int count = 0; count < BlockCount; count++) {
+					uint macroBlockEmpty = ReadStreamData(1);
+
+					if (macroBlockEmpty.intValue() == (0)) {
+						uint acCoefficients = ReadStreamData(8);
+
+						blockY0HasAcComponents = acCoefficients.shiftRight(0)
+								.and(1).intValue() == 1;
+						blockY1HasAcComponents = acCoefficients.shiftRight(1)
+								.and(1).intValue() == 1;
+						blockY2HasAcComponents = acCoefficients.shiftRight(2)
+								.and(1).intValue() == 1;
+						blockY3HasAcComponents = acCoefficients.shiftRight(3)
+								.and(1).intValue() == 1;
+						blockCbHasAcComponents = acCoefficients.shiftRight(4)
+								.and(1).intValue() == 1;
+						blockCrHasAcComponents = acCoefficients.shiftRight(5)
+								.and(1).intValue() == 1;
+
+						if (acCoefficients.shiftRight(6).and(1).intValue() == 1) {
+							uint quantizerMode = ReadStreamData(2);
+							QuantizerMode = (int) ((quantizerMode.intValue() < 2) ? quantizerMode
+									.flipBits() : quantizerMode.intValue());
+						}
+
+						// /#//#region Block Y0
+
+						GetBlockBytes(blockY0HasAcComponents);
+						InverseTransform(count, 0);
+
+						// /#//#endregion
+
+						// /#//#region Block Y1
+
+						GetBlockBytes(blockY1HasAcComponents);
+						InverseTransform(count, 1);
+
+						// /#//#endregion
+
+						// /#//#region Block Y2
+
+						GetBlockBytes(blockY2HasAcComponents);
+						InverseTransform(count, 2);
+
+						// /#//#endregion
+
+						// /#//#region Block Y3
+
+						GetBlockBytes(blockY3HasAcComponents);
+						InverseTransform(count, 3);
+
+						// /#//#endregion
+
+						// /#//#region Block Cb
+
+						GetBlockBytes(blockCbHasAcComponents);
+						InverseTransform(count, 4);
+
+						// /#//#endregion
+
+						// /#//#region Block Cr
+
+						GetBlockBytes(blockCrHasAcComponents);
+						InverseTransform(count, 5);
+
+						// /#//#endregion
+					}
+				}
+
+				ComposeImageSlice();
+			}
+		}
+
+		// System.out.println("PixelData Length " + PixelData.length);
+		// System.out.println("PixelRowSize " + PixelRowSize);
+		// System.out.println("Width " + Width);
+		// System.out.println("Height " + Height);
+		// System.out.println("Length/PixelRowSize "
+		// + (PixelData.length / PixelRowSize));
+
+		/*
+		 * unsafe { fixed (ushort* pixelData = PixelData) { IntPtr pixelDataPtr
+		 * = (IntPtr)pixelData; ImageSource.Lock();
+		 * CopyMemory(ImageSource.BackBuffer, pixelDataPtr, PixelData.Length *
+		 * 2); ImageSource.AddDirtyRect(Rectangle); ImageSource.Unlock(); } }
+		 * 
+		 * if (ImageComplete != null) { ImageComplete(this, new
+		 * ImageCompleteEventArgs(ImageSource)); }
+		 */
+	}
+
+	private void ReadHeader() {
+		uint code = new uint(0);
+		uint startCode = new uint(0);
+
+		AlignStreamData();
+
+		code = ReadStreamData(22);
+
+		startCode = new uint(code.and(~0x1F));
+
+		if (startCode.intValue() == 32) {
+			if (((code.and(0x1F).intValue()) == 0x1F)) {
+				PictureComplete = true;
+			} else {
+				if (SliceIndex++ == 0) {
+					PictureFormat = (int) ReadStreamData(2).intValue();
+					Resolution = (int) ReadStreamData(3).intValue();
+					PictureType = (int) ReadStreamData(3).intValue();
+					QuantizerMode = (int) ReadStreamData(5).intValue();
+					FrameIndex = (int) ReadStreamData(32).intValue();
+
+					switch (PictureFormat) {
+					case (int) PictureFormats.Cif:
+						Width = _WidthCif << Resolution - 1;
+						Height = _HeightCif << Resolution - 1;
+						break;
+					case (int) PictureFormats.Vga:
+						Width = _WidthVga << Resolution - 1;
+						Height = _HeightVga << Resolution - 1;
+						break;
+					}
+
+					// We assume two bytes per pixel (RGB 565)
+					PixelRowSize = Width << 1;
+
+					SliceCount = Height >> 4;
+					BlockCount = Width >> 4;
+
+					if (ImageSlice == null) {
+						ImageSlice = new ImageSlice(BlockCount);
+						PixelData = new uint[Width * Height];
+						// ImageSource = new WriteableBitmap(Width, Height, 96,
+						// 96, PixelFormats.Bgr565, null);
+						// Rectangle = new Int32Rect(0, 0, Width, Height);
+					} else {
+						if (ImageSlice.MacroBlocks.length != BlockCount) {
+							ImageSlice = new ImageSlice(BlockCount);
+							PixelData = new uint[Width * Height];
+							// ImageSource = new WriteableBitmap(Width, Height,
+							// 96, 96, PixelFormats.Bgr565, null);
+							// Rectangle = new Int32Rect(0, 0, Width, Height);
+						}
+					}
+				} else {
+					QuantizerMode = (int) ReadStreamData(5).intValue();
+				}
+			}
+		}
+	}
+
+	private void GetBlockBytes(boolean acCoefficientsAvailable) {
+		int[] run = new int[] { 0 };
+		int[] level = new int[] { 0 };
+		int zigZagPosition = 0;
+		int matrixPosition = 0;
+		boolean[] last = new boolean[] { false };
+
+		for (int i = 0; i < dataBlockBuffer.length; i++) {
+			dataBlockBuffer[i] = 0;
+		}
+		// Array.Clear(dataBlockBuffer, 0, dataBlockBuffer.length);
+
+		uint dcCoefficient = ReadStreamData(10);
+
+		if (QuantizerMode == _TableQuantization) {
+			dataBlockBuffer[0] = (short) (dcCoefficient
+					.times(quantizerValues[0]));
+
+			if (acCoefficientsAvailable) {
+				DecodeFieldBytes(run, level, last);
+
+				while (!last[0]) {
+					zigZagPosition += run[0] + 1;
+					matrixPosition = zigZagPositions[zigZagPosition];
+					level[0] *= quantizerValues[matrixPosition];
+					dataBlockBuffer[matrixPosition] = (short) level[0];
+					DecodeFieldBytes(run, level, last);
+				}
+			}
+		} else {
+			// Currently not implemented.
+			throw new RuntimeException(
+					"ant quantizer mode is not yet implemented.");
+		}
+	}
+
+	private void DecodeFieldBytes(int[] run, int[] level, boolean[] last) {
+		uint streamCode = new uint(0);
+
+		int streamLength = 0;
+		;
+		int zeroCount = 0;
+		int temp = 0;
+		int sign = 0;
+
+		// Use the RLE and Huffman dictionaries to understand this code
+		// fragment. You can find
+		// them in the developers guide on page 34.
+		// The bits in the data are actually composed of two kinds of fields:
+		// - run fields - this field contains information on the number of
+		// consecutive zeros.
+		// - level fields - this field contains the actual non zero value which
+		// can be negative or positive.
+		// First we extract the run field info and then the level field info.
+
+		streamCode = PeekStreamData(ImageStream, 32);
+
+		// /#//#region Determine number of consecutive zeros in zig zag. (a.k.a
+		// 'run' field info)
+
+		// Suppose we have following bit sequence:
+		// 00001111.....
+		// 1 - Count the number of leading zeros -> 4
+		// Coarse value lookup is thus 00001
+		// 2 - Lookup the additional value, for coarse value 00001 this is 3
+		// addtional bits
+		// 3 - Calculate value of run, for coarse value 00001 this is (111) + 8
+
+		zeroCount = CountLeadingZeros(streamCode); // - (1)
+		streamCode.shiftLeftEquals(zeroCount + 1); // - (2) -> shift left to get
+		// rid of the coarse value
+		streamLength += zeroCount + 1; // - position bit pointer to keep track
+		// off how many bits to consume later on
+		// the stream.
+
+		if (zeroCount > 1) {
+			temp = (streamCode.shiftRight(32 - (zeroCount - 1))).intValue(); // -
+			// (2)
+			// ->
+			// shift
+			// right
+			// to
+			// determine
+			// the
+			// addtional
+			// bits
+			// (number
+			// of
+			// additional
+			// bits
+			// is
+			// zerocount
+			// - 1)
+			streamCode.shiftLeftEquals(zeroCount - 1); // - shift all of the run
+			// bits out of the way
+			// so the first bit is
+			// points to the first
+			// bit of the level
+			// field.
+			streamLength += zeroCount - 1;// - position bit pointer to keep
+			// track off how many bits to
+			// consume later on the stream.
+			run[0] = temp + (1 << (zeroCount - 1)); // - (3) -> calculate run
+			// value
+		} else {
+			run[0] = zeroCount;
+		}
+
+		// /#//#endregion
+
+		// /#//#region Determine non zero value. (a.k.a 'level' field info)
+
+		// Suppose we have following bit sequence:
+		// 000011111.....
+		// 1 - Count the number of leading zeros -> 4
+		// Coarse value lookup is thus 00001
+		// 2 - Lookup the additional value, for coarse value 00001 this is 4
+		// addtional bits (last bit is sign bit)
+		// 3 - Calculate value of run, for coarse value 00001 this is (xxx) + 8,
+		// multiply by sign
+
+		zeroCount = CountLeadingZeros(streamCode);
+		streamCode.shiftLeftEquals(zeroCount + 1); // - (1)
+		streamLength += zeroCount + 1; // - position bit pointer to keep track
+		// off how many bits to consume later on
+		// the stream.
+
+		if (zeroCount == 1) {
+			// If coarse value is 01 according to the Huffman dictionary this
+			// means EOB, so there is
+			// no run and level and we indicate this by setting last to true;
+			run[0] = 0;
+			last[0] = true;
+		} else {
+			if (zeroCount == 0) {
+				zeroCount = 1;
+				temp = 1;
+			}
+
+			streamLength += zeroCount;// - position bit pointer to keep track
+			// off how many bits to consume later on
+			// the stream.
+			streamCode.shiftRightEquals(32 - zeroCount);// - (2) -> shift right
+			// to determine the
+			// addtional bits
+			// (number of additional
+			// bits is zerocount)
+			// sign = (sbyte)(streamCode & 1); // determine sign, last bit is
+			// sign
+			sign = (int) (streamCode.and(1).intValue()); // determine sign, last
+			// bit is sign
+
+			if (zeroCount != 0) {
+				// temp = (sbyte)(streamCode >> 1); // take into account that
+				// last bit is sign, so shift it out of the way
+				// temp += (sbyte)(1 << (zeroCount - 1)); // - (3) -> calculate
+				// run value without sign
+				temp = (streamCode.shiftRight(1)).intValue(); // take into
+				// account
+				// that last bit is
+				// sign, so shift it
+				// out of the way
+				temp += (int) (1 << (zeroCount - 1)); // - (3) -> calculate run
+				// value without sign
+			}
+
+			level[0] = (sign == 1) ? -temp : temp; // - (3) -> calculate run
+			// value with sign
+			last[0] = false;
+		}
+
+		// /#//#endregion
+
+		ReadStreamData(streamLength);
+	}
+
+	int numCalls = 0;
+
+	private uint ReadStreamData(int count) {
+		uint data = new uint(0);
+
+		while (count > (32 - StreamFieldBitIndex)) {
+			data = (data.shiftLeft((int) (32 - StreamFieldBitIndex))
+					.or(StreamField.shiftRight(StreamFieldBitIndex)));
+
+			count -= 32 - StreamFieldBitIndex;
+
+			StreamField = new uint(ImageStream, StreamIndex * 4);
+
+			StreamFieldBitIndex = 0;
+			StreamIndex++;
+		}
+
+		if (count > 0) {
+			data = data.shiftLeft(count).or(StreamField.shiftRight(32 - count));
+
+			StreamField.shiftLeftEquals(count);
+			StreamFieldBitIndex += count;
+		}
+
+		numCalls++;
+		// System.out.println("ReadStreamData " + data + " " + numCalls + " " +
+		// count);
+
+		return data;
+	}
+
+	private uint PeekStreamData(byte[] stream, int count) {
+		uint data = new uint(0);
+		uint streamField = StreamField;
+		int streamFieldBitIndex = StreamFieldBitIndex;
+
+		while (count > (32 - streamFieldBitIndex)
+				&& StreamIndex < (ImageStream.length >> 2)) {
+			data = (data.shiftLeft(32 - streamFieldBitIndex)).or(streamField
+					.shiftRight(streamFieldBitIndex));
+
+			count -= 32 - streamFieldBitIndex;
+
+			streamField = new uint(stream, StreamIndex * 4);
+			streamFieldBitIndex = 0;
+		}
+
+		if (count > 0) {
+			data = data.shiftLeft(count).or(
+					streamField.shiftRight((32 - count)));
+		}
+
+		return data;
+	}
+
+	private void AlignStreamData() {
+		int alignedLength;
+		int actualLength;
+
+		actualLength = StreamFieldBitIndex;
+
+		if (actualLength > 0) {
+			alignedLength = (actualLength & ~7);
+			if (alignedLength != actualLength) {
+				alignedLength += 0x08;
+				StreamField.shiftLeftEquals(alignedLength - actualLength);
+				StreamFieldBitIndex = alignedLength;
+			}
+		}
+	}
+
+	// Blockline:
+	// _______
+	// | 1 | 2 |
+	// |___|___| Y
+	// | 3 | 4 |
+	// |___|___|
+	// ___
+	// | 5 |
+	// |___| Cb
+	// ___
+	// | 6 |
+	// |___| Cr
+	//
+	// Layout in memory
+	// _______________________
+	// | 1 | 2 | 3 | 4 | 5 | 6 | ...
+	// |___|___|___|___|___|___|
+	//
+
+	// Example, suppose the six data sub blocks are as follows:
+
+	// ==============Y0============== ==============Y1==============
+	// ==============Y2============== ==============Y3==============
+
+	// 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7,
+	// 0, 1, 2, 3, 4, 5, 6, 7,
+	// 8, 9, 10, 11, 12, 13, 14, 15, 8, 9, 10, 11, 12, 13, 14, 15, 8, 9, 10, 11,
+	// 12, 13, 14, 15, 8, 9, 10, 11, 12, 13, 14, 15,
+	// 16, 17, 18, 19, 20, 21, 22, 23, 16, 17, 18, 19, 20, 21, 22, 23, 16, 17,
+	// 18, 19, 20, 21, 22, 23, 16, 17, 18, 19, 20, 21, 22, 23,
+	// 24, 25, 26, 27, 28, 29, 30, 31, 24, 25, 26, 27, 28, 29, 30, 31, 24, 25,
+	// 26, 27, 28, 29, 30, 31, 24, 25, 26, 27, 28, 29, 30, 31,
+	// 32, 33, 34, 35, 36, 37, 38, 39, 32, 33, 34, 35, 36, 37, 38, 39, 32, 33,
+	// 34, 35, 36, 37, 38, 39, 32, 33, 34, 35, 36, 37, 38, 39,
+	// 40, 41, 42, 43, 44, 45, 46, 47, 40, 41, 42, 43, 44, 45, 46, 47, 40, 41,
+	// 42, 43, 44, 45, 46, 47, 40, 41, 42, 43, 44, 45, 46, 47,
+	// 48, 49, 50, 51, 52, 53, 54, 55, 48, 49, 50, 51, 52, 53, 54, 55, 48, 49,
+	// 50, 51, 52, 53, 54, 55, 48, 49, 50, 51, 52, 53, 54, 55,
+	// 56, 57, 58, 59, 60, 61, 62, 63, 56, 57, 58, 59, 60, 61, 62, 63, 56, 57,
+	// 58, 59, 60, 61, 62, 63, 56, 57, 58, 59, 60, 61, 62, 63
+
+	// ==============Cb============== ==============Cr==============
+
+	// 0, 1, 2, 3, | 4, 5, 6, 7, 0, 1, 2, 3, | 4, 5, 6, 7,
+	// 8, 9, 10, 11, | 12, 13, 14, 15, 8, 9, 10, 11, | 12, 13, 14, 15,
+	// 16, 17, 18, 19, | 20, 21, 22, 23, 16, 17, 18, 19, | 20, 21, 22, 23,
+	// 24, 25, 26, 27, | 28, 29, 30, 31, 24, 25, 26, 27, | 28, 29, 30, 31,
+	// ----------------| --------------- --------------- | ---------------
+	// 32, 33, 34, 35, | 36, 37, 38, 39, 32, 33, 34, 35, | 36, 37, 38, 39,
+	// 40, 41, 42, 43, | 44, 45, 46, 47, 40, 41, 42, 43, | 44, 45, 46, 47,
+	// 48, 49, 50, 51, | 52, 53, 54, 55, 48, 49, 50, 51, | 52, 53, 54, 55,
+	// 56, 57, 58, 59, | 60, 61, 62, 63, 56, 57, 58, 59, | 60, 61, 62, 63,
+
+	// Pixel Matrix
+
+	// 0, 1, 2, 3, 4, 5, 6, 7, | 8, 9, 10, 11, 12, 13, 14, 15,
+	// 16, 17, 18, 19, 20, 21, 22, 23, | 24, 25, 26, 27, 28, 29, 30, 31,
+	// 32, 33, 34, 35, 36, 37, 38, 39, | 40, 41, 42, 43, 44, 45, 46, 47,
+	// 48, 49, 50, 51, 52, 53, 54, 55, | 56, 57, 58, 59, 60, 61, 62, 63,
+	// 64, 65, 66, 67, 68, 69, 70, 71, | 72, 73, 74, 75, 76, 77, 78, 79,
+	// 80, 81, 82, 83, 84, 85, 86, 87, | 88, 89, 90, 91, 92, 93, 94, 95,
+	// 96, 97, 98, 99, 100, 101, 102, 103, | 104, 105, 106, 107, 108, 109, 110,
+	// 111,
+	// 112, 113, 114, 115, 116, 117, 118, 119, | 120, 121, 122, 123, 124, 125,
+	// 126, 127,
+	// ----------------------------------------|---------------------------------------
+	// 128, 129, 130, 131, 132, 133, 134, 135, | 136, 137, 138, 139, 140, 141,
+	// 142, 143,
+	// 144, 145, 146, 147, 148, 149, 150, 151, | 152, 153, 154, 155, 156, 157,
+	// 158, 159,
+	// 160, 161, 162, 163, 164, 165, 166, 167, | 168, 169, 170, 171, 172, 173,
+	// 174, 175,
+	// 176, 177, 178, 179, 180, 181, 182, 183, | 184, 185, 186, 187, 188, 189,
+	// 190, 191,
+	// 192, 193, 194, 195, 196, 197, 198, 199, | 200, 201, 202, 203, 204, 205,
+	// 206, 207,
+	// 208, 209, 210, 211, 212, 213, 214, 215, | 216, 217, 218, 219, 220, 221,
+	// 222, 223,
+	// 224, 225, 226, 227, 228, 229, 230, 231, | 232, 233, 234, 235, 236, 237,
+	// 238, 239,
+	// 240, 241, 242, 243, 244, 245, 246, 247, | 248, 249, 250, 251, 252, 253,
+	// 254, 255,
+
+	// The four Luma 8x8 matrices (quadrants Y0, Y1, Y2, Y3) form the basis of
+	// the final 16x16 pixel matrix.
+	// The two Croma 8x8 matrices are used to calculate the actual RGB value of
+	// the pixel (RGB565, each pixel is represented by two bytes)
+
+	// Each processing loop processes from each Luma matrix two rows. In each
+	// 'two row' loop the rows are processed
+	// by two columns.
+
+	// First Loop will take (assume there is only one pixel matrix to fill):
+
+	// Quadrant 1
+	// From Cb -> 0
+	// From Cr -> 0
+	// From Y0 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 0, 16 and 1 and 17
+
+	// Quadrant 2
+	// From Cb -> 4
+	// From Cr -> 4
+	// From Y1 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 8, 24 and 9 and 25
+
+	// Quadrant 3
+	// From Cb -> 32
+	// From Cr -> 32
+	// From Y2 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 128, 144 and 129 and 145
+
+	// Quadrant 4
+	// From Cb -> 36
+	// From Cr -> 36
+	// From Y3 -> 0, 8 and 1, 9 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 136, 152 and 137 and 153
+
+	// Second Loop will take (assume there is only one pixel matrix to fill):
+
+	// Quadrant 1
+	// From Cb -> 1
+	// From Cr -> 1
+	// From Y0 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 2, 18 and 3 and 19
+
+	// Quadrant 2
+	// From Cb -> 5
+	// From Cr -> 5
+	// From Y1 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 10, 26 and 11 and 27
+
+	// Quadrant 3
+	// From Cb -> 33
+	// From Cr -> 33
+	// From Y2 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 130, 146 and 131 and 147
+	// Quadrant 4
+	// From Cb -> 37
+	// From Cr -> 37
+	// From Y3 -> 2, 10 and 3, 11 - use Cb and Cr to calculate RGB and place in
+	// pixel matrix in 138, 154 and 139 and 155
+
+	// We need third and fourth loop to complete first two lines of the luma
+	// blocks. At this time we
+	// have written 64 pixels to the pixel matrix.
+
+	// These four loops have to be repeated 4 more times (4 * 64 = 256) to fill
+	// complete pixel matrix.
+
+	// Remark the offsets to use in the pixel matrix have to take into account
+	// that an GroupOfBlocks contains multiple pixel matrices.
+	// So to calculate the real index we have to take that also into account
+	// (BlockCount)
+
+	private void ComposeImageSlice() {
+		int u, ug, ub;
+		int v, vg, vr;
+		int r, g, b;
+
+		int lumaElementIndex1 = 0;
+		int lumaElementIndex2 = 0;
+		int chromaOffset = 0;
+
+		int dataIndex1 = 0;
+		int dataIndex2 = 0;
+
+		int lumaElementValue1 = 0;
+		int lumaElementValue2 = 0;
+		int chromaBlueValue = 0;
+		int chromaRedValue = 0;
+
+		int[] cromaQuadrantOffsets = new int[] { 0, 4, 32, 36 };
+		int[] pixelDataQuadrantOffsets = new int[] { 0, _BlockWidth,
+				Width * _BlockWidth, (Width * _BlockWidth) + _BlockWidth };
+
+		int imageDataOffset = (SliceIndex - 1) * Width * 16;
+
+		for (MacroBlock macroBlock : ImageSlice.MacroBlocks) {
+			for (int verticalStep = 0; verticalStep < _BlockWidth / 2; verticalStep++) {
+				chromaOffset = verticalStep * _BlockWidth;
+				lumaElementIndex1 = verticalStep * _BlockWidth * 2;
+				lumaElementIndex2 = lumaElementIndex1 + _BlockWidth;
+
+				dataIndex1 = imageDataOffset + (2 * verticalStep * Width);
+				dataIndex2 = dataIndex1 + Width;
+
+				for (int horizontalStep = 0; horizontalStep < _BlockWidth / 2; horizontalStep++) {
+					for (int quadrant = 0; quadrant < 4; quadrant++) {
+						int chromaIndex = chromaOffset
+								+ cromaQuadrantOffsets[quadrant]
+								+ horizontalStep;
+						chromaBlueValue = macroBlock.DataBlocks[4][chromaIndex];
+						chromaRedValue = macroBlock.DataBlocks[5][chromaIndex];
+
+						u = chromaBlueValue - 128;
+						ug = 88 * u;
+						ub = 454 * u;
+
+						v = chromaRedValue - 128;
+						vg = 183 * v;
+						vr = 359 * v;
+
+						for (int pixel = 0; pixel < 2; pixel++) {
+							int deltaIndex = 2 * horizontalStep + pixel;
+							lumaElementValue1 = macroBlock.DataBlocks[quadrant][lumaElementIndex1
+									+ deltaIndex] << 8;
+							lumaElementValue2 = macroBlock.DataBlocks[quadrant][lumaElementIndex2
+									+ deltaIndex] << 8;
+
+							r = Saturate5(lumaElementValue1 + vr);
+							g = Saturate6(lumaElementValue1 - ug - vg);
+							b = Saturate5(lumaElementValue1 + ub);
+
+							PixelData[dataIndex1
+									+ pixelDataQuadrantOffsets[quadrant]
+									+ deltaIndex] = MakeRgb(r, g, b);
+
+							r = Saturate5(lumaElementValue2 + vr);
+							g = Saturate6(lumaElementValue2 - ug - vg);
+							b = Saturate5(lumaElementValue2 + ub);
+
+							PixelData[dataIndex2
+									+ pixelDataQuadrantOffsets[quadrant]
+									+ deltaIndex] = MakeRgb(r, g, b);
+						}
+					}
+				}
+			}
+
+			imageDataOffset += 16;
+		}
+	}
+
+	private int Saturate5(int x) {
+		if (x < 0) {
+			x = 0;
+		}
+
+		x >>= 11;
+
+		return (x > 0x1F) ? 0x1F : x;
+	}
+
+	private int Saturate6(int x) {
+		if (x < 0) {
+			x = 0;
+		}
+
+		x >>= 10;
+
+		return x > 0x3F ? 0x3F : x;
+	}
+
+	private uint MakeRgb(int r, int g, int b) {
+		r <<= 2;
+		g <<= 1;
+		b <<= 2;
+
+		uint ru = new uint(r);
+		uint gu = new uint(g);
+		uint bu = new uint(b);
+
+		uint retval = ru.shiftLeft(16);
+		retval = retval.or(gu.shiftLeft(8));
+		retval = retval.or(bu);
+
+		return retval;
+		// return new newUint((r << 16) | (g << 8) | b);
+	}
+
+	private int CountLeadingZeros(uint value) {
+		int accum = 0;
+
+		accum += clzlut[value.shiftRight(24).intValue()];
+		if (accum == 8) {
+			accum += clzlut[(value.shiftRight(16).intValue()) & 0xFF];
+		}
+		if (accum == 16) {
+			accum += clzlut[(value.shiftRight(8).intValue()) & 0xFF];
+		}
+		if (accum == 24) {
+			accum += clzlut[value.intValue() & 0xFF];
+		}
+
+		return accum;
+	}
+
+	// /#//#region Dct Methods
+
+	void InverseTransform(int macroBlockIndex, int dataBlockIndex) {
+		int[] workSpace = new int[64];
+		short[] data = new short[64];
+
+		int z1, z2, z3, z4, z5;
+		int tmp0, tmp1, tmp2, tmp3;
+		int tmp10, tmp11, tmp12, tmp13;
+
+		int pointer = 0;
+
+		for (int index = 8; index > 0; index--) {
+			if (dataBlockBuffer[pointer + 8] == 0
+					&& dataBlockBuffer[pointer + 16] == 0
+					&& dataBlockBuffer[pointer + 24] == 0
+					&& dataBlockBuffer[pointer + 32] == 0
+					&& dataBlockBuffer[pointer + 40] == 0
+					&& dataBlockBuffer[pointer + 48] == 0
+					&& dataBlockBuffer[pointer + 56] == 0) {
+				int dcValue = dataBlockBuffer[pointer] << PASS1_BITS;
+
+				workSpace[pointer + 0] = dcValue;
+				workSpace[pointer + 8] = dcValue;
+				workSpace[pointer + 16] = dcValue;
+				workSpace[pointer + 24] = dcValue;
+				workSpace[pointer + 32] = dcValue;
+				workSpace[pointer + 40] = dcValue;
+				workSpace[pointer + 48] = dcValue;
+				workSpace[pointer + 56] = dcValue;
+
+				pointer++;
+				continue;
+			}
+
+			z2 = dataBlockBuffer[pointer + 16];
+			z3 = dataBlockBuffer[pointer + 48];
+
+			z1 = (z2 + z3) * FIX_0_541196100;
+			tmp2 = z1 + z3 * -FIX_1_847759065;
+			tmp3 = z1 + z2 * FIX_0_765366865;
+
+			z2 = dataBlockBuffer[pointer];
+			z3 = dataBlockBuffer[pointer + 32];
+
+			tmp0 = (z2 + z3) << _BITS;
+			tmp1 = (z2 - z3) << _BITS;
+
+			tmp10 = tmp0 + tmp3;
+			tmp13 = tmp0 - tmp3;
+			tmp11 = tmp1 + tmp2;
+			tmp12 = tmp1 - tmp2;
+
+			tmp0 = dataBlockBuffer[pointer + 56];
+			tmp1 = dataBlockBuffer[pointer + 40];
+			tmp2 = dataBlockBuffer[pointer + 24];
+			tmp3 = dataBlockBuffer[pointer + 8];
+
+			z1 = tmp0 + tmp3;
+			z2 = tmp1 + tmp2;
+			z3 = tmp0 + tmp2;
+			z4 = tmp1 + tmp3;
+			z5 = (z3 + z4) * FIX_1_175875602;
+
+			tmp0 = tmp0 * FIX_0_298631336;
+			tmp1 = tmp1 * FIX_2_053119869;
+			tmp2 = tmp2 * FIX_3_072711026;
+			tmp3 = tmp3 * FIX_1_501321110;
+			z1 = z1 * -FIX_0_899976223;
+			z2 = z2 * -FIX_2_562915447;
+			z3 = z3 * -FIX_1_961570560;
+			z4 = z4 * -FIX_0_390180644;
+
+			z3 += z5;
+			z4 += z5;
+
+			tmp0 += z1 + z3;
+			tmp1 += z2 + z4;
+			tmp2 += z2 + z3;
+			tmp3 += z1 + z4;
+
+			workSpace[pointer + 0] = ((tmp10 + tmp3 + (1 << F1)) >> F2);
+			workSpace[pointer + 56] = ((tmp10 - tmp3 + (1 << F1)) >> F2);
+			workSpace[pointer + 8] = ((tmp11 + tmp2 + (1 << F1)) >> F2);
+			workSpace[pointer + 48] = ((tmp11 - tmp2 + (1 << F1)) >> F2);
+			workSpace[pointer + 16] = ((tmp12 + tmp1 + (1 << F1)) >> F2);
+			workSpace[pointer + 40] = ((tmp12 - tmp1 + (1 << F1)) >> F2);
+			workSpace[pointer + 24] = ((tmp13 + tmp0 + (1 << F1)) >> F2);
+			workSpace[pointer + 32] = ((tmp13 - tmp0 + (1 << F1)) >> F2);
+
+			pointer++;
+		}
+
+		pointer = 0;
+
+		for (int index = 0; index < 8; index++) {
+			z2 = workSpace[pointer + 2];
+			z3 = workSpace[pointer + 6];
+
+			z1 = (z2 + z3) * FIX_0_541196100;
+			tmp2 = z1 + z3 * -FIX_1_847759065;
+			tmp3 = z1 + z2 * FIX_0_765366865;
+
+			tmp0 = (workSpace[pointer + 0] + workSpace[pointer + 4]) << _BITS;
+			tmp1 = (workSpace[pointer + 0] - workSpace[pointer + 4]) << _BITS;
+
+			tmp10 = tmp0 + tmp3;
+			tmp13 = tmp0 - tmp3;
+			tmp11 = tmp1 + tmp2;
+			tmp12 = tmp1 - tmp2;
+
+			tmp0 = workSpace[pointer + 7];
+			tmp1 = workSpace[pointer + 5];
+			tmp2 = workSpace[pointer + 3];
+			tmp3 = workSpace[pointer + 1];
+
+			z1 = tmp0 + tmp3;
+			z2 = tmp1 + tmp2;
+			z3 = tmp0 + tmp2;
+			z4 = tmp1 + tmp3;
+
+			z5 = (z3 + z4) * FIX_1_175875602;
+
+			tmp0 = tmp0 * FIX_0_298631336;
+			tmp1 = tmp1 * FIX_2_053119869;
+			tmp2 = tmp2 * FIX_3_072711026;
+			tmp3 = tmp3 * FIX_1_501321110;
+			z1 = z1 * -FIX_0_899976223;
+			z2 = z2 * -FIX_2_562915447;
+			z3 = z3 * -FIX_1_961570560;
+			z4 = z4 * -FIX_0_390180644;
+
+			z3 += z5;
+			z4 += z5;
+
+			tmp0 += z1 + z3;
+			tmp1 += z2 + z4;
+			tmp2 += z2 + z3;
+			tmp3 += z1 + z4;
+
+			data[pointer + 0] = (short) ((tmp10 + tmp3) >> F3);
+			data[pointer + 7] = (short) ((tmp10 - tmp3) >> F3);
+			data[pointer + 1] = (short) ((tmp11 + tmp2) >> F3);
+			data[pointer + 6] = (short) ((tmp11 - tmp2) >> F3);
+			data[pointer + 2] = (short) ((tmp12 + tmp1) >> F3);
+			data[pointer + 5] = (short) ((tmp12 - tmp1) >> F3);
+			data[pointer + 3] = (short) ((tmp13 + tmp0) >> F3);
+			data[pointer + 4] = (short) ((tmp13 - tmp0) >> F3);
+
+			pointer += 8;
+		}
+
+		for (int i = 0; i < data.length; i++) {
+			ImageSlice.MacroBlocks[macroBlockIndex].DataBlocks[dataBlockIndex][i] = data[i];
+		}
+		/*
+		 * unsafe { fixed (short* source = data) fixed (short* destination =
+		 * ImageSlice.MacroBlocks[macroBlockIndex].DataBlocks[dataBlockIndex]) {
+		 * IntPtr sourcePtr = (IntPtr)source; IntPtr destinationPtr =
+		 * (IntPtr)destination; CopyMemory(destinationPtr, sourcePtr,
+		 * data.Length * 2); } }
+		 */
+	}
+}
+
+// /#//#endregion
+
+// /#//#endregion

src/com/shigeodayo/ardrone/video/VideoManager.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.video;
+
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.manager.AbstractManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+public abstract class VideoManager extends AbstractManager {
+	protected CommandManager manager = null;
+
+	protected ImageListener listener = null;
+
+	public VideoManager(InetAddress inetaddr, CommandManager manager) {
+		this.inetaddr = inetaddr;
+		this.manager = manager;
+	}
+	
+	public void setImageListener(ImageListener listener) {
+		this.listener = listener;
+	}
+
+	public void removeImageListener() {
+		listener = null;
+	}
+	
+	
+	protected void setVideoPort() {
+		ticklePort(ARDroneConstants.VIDEO_PORT);
+		manager.enableVideoData();
+		ticklePort(ARDroneConstants.VIDEO_PORT);
+		manager.disableAutomaticVideoBitrate();		
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/video/VideoManager1.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.video;
+
+import java.awt.image.BufferedImage;
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.InetAddress;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+/**
+ * VideoManager for AR.Drone 1.0
+ * 
+ * @author shigeo
+ *
+ */
+public class VideoManager1 extends VideoManager {
+
+	private ReadRawFileImage rrfi = null;
+
+	public VideoManager1(InetAddress inetaddr, CommandManager manager) {
+		super(inetaddr, manager);
+		rrfi = new ReadRawFileImage();
+		
+		//System.out.println("video manager 1");
+	}
+
+	@Override
+	public void run() {
+		setVideoPort();
+		
+		byte[] buf = new byte[153600];
+		DatagramPacket packet = new DatagramPacket(buf, buf.length);
+		BufferedImage image = null;
+		while (true) {
+			try {
+				ticklePort(ARDroneConstants.VIDEO_PORT);
+				socket.receive(packet);
+				image = rrfi.readUINT_RGBImage(buf);
+				if (listener != null) {
+					listener.imageUpdated(image);
+				}
+			} catch (IOException e) {
+				e.printStackTrace();
+			}
+		}
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/video/VideoManager2.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package com.shigeodayo.ardrone.video;
+
+import java.awt.image.BufferedImage;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.net.InetAddress;
+import java.net.Socket;
+
+import com.shigeodayo.ardrone.command.CommandManager;
+import com.xuggle.xuggler.Global;
+import com.xuggle.xuggler.ICodec;
+import com.xuggle.xuggler.IContainer;
+import com.xuggle.xuggler.IPacket;
+import com.xuggle.xuggler.IPixelFormat;
+import com.xuggle.xuggler.IStream;
+import com.xuggle.xuggler.IStreamCoder;
+import com.xuggle.xuggler.IVideoPicture;
+import com.xuggle.xuggler.IVideoResampler;
+import com.xuggle.xuggler.Utils;
+
+/**
+ * VideoManager for AR.Drone 2.0
+ *
+ * Thank you Dirk !!
+ * http://vsis-www.informatik.uni-hamburg.de/projects/yadrone/
+ *
+ */
+public class VideoManager2 extends VideoManager {
+
+	// using TCP
+	private Socket socket = null;
+
+	public VideoManager2(InetAddress inetaddr, CommandManager manager) {
+		super(inetaddr, manager);
+		
+		//System.out.println("video manager 2");
+	}
+
+	@Override
+	public void run() {
+		setVideoPort();
+		
+		decode();
+	}
+
+	private void decode() {
+		// Let's make sure that we can actually convert video pixel formats.
+		/*
+		 * if (!IVideoResampler
+		 * .isSupported(IVideoResampler.Feature.FEATURE_COLORSPACECONVERSION))
+		 * throw new RuntimeException("you must install the GPL version" +
+		 * " of Xuggler (with IVideoResampler support) for " +
+		 * "this demo to work");
+		 */
+
+		// Create a Xuggler container object
+		IContainer container = IContainer.make();
+		// Open up the container
+		try {
+			if (container.open(socket.getInputStream(), null) < 0)
+				throw new IllegalArgumentException("could not open inpustream");
+		} catch (IOException e1) {
+			e1.printStackTrace();
+		}
+
+		// query how many streams the call to open found
+		int numStreams = container.getNumStreams();
+
+		// and iterate through the streams to find the first video stream
+		int videoStreamId = -1;
+		IStreamCoder videoCoder = null;
+		for (int i = 0; i < numStreams; i++) {
+			// Find the stream object
+			IStream stream = container.getStream(i);
+			// Get the pre-configured decoder that can decode this stream;
+			IStreamCoder coder = stream.getStreamCoder();
+
+			if (coder.getCodecType() == ICodec.Type.CODEC_TYPE_VIDEO) {
+				videoStreamId = i;
+				videoCoder = coder;
+				break;
+			}
+		}
+		if (videoStreamId == -1)
+			throw new RuntimeException("could not find video stream");
+
+		/*
+		 * Now we have found the video stream in this file. Let's open up our
+		 * decoder so it can do work.
+		 */
+		if (videoCoder.open() < 0)
+			throw new RuntimeException(
+					"could not open video decoder for container");
+
+		IVideoResampler resampler = null;
+		if (videoCoder.getPixelType() != IPixelFormat.Type.BGR24) {
+			// if this stream is not in BGR24, we're going to need to
+			// convert it. The VideoResampler does that for us.
+			resampler = IVideoResampler.make(videoCoder.getWidth(),
+					videoCoder.getHeight(), IPixelFormat.Type.BGR24,
+					videoCoder.getWidth(), videoCoder.getHeight(),
+					videoCoder.getPixelType());
+			if (resampler == null)
+				throw new RuntimeException(
+						"could not create color space resampler.");
+		}
+
+		/*
+		 * Now, we start walking through the container looking at each packet.
+		 */
+		IPacket packet = IPacket.make();
+		long firstTimestampInStream = Global.NO_PTS;
+		long systemClockStartTime = 0;
+		while (container.readNextPacket(packet) >= 0) {
+			/*
+			 * Now we have a packet, let's see if it belongs to our video stream
+			 */
+			if (packet.getStreamIndex() == videoStreamId) {
+				/*
+				 * We allocate a new picture to get the data out of Xuggler
+				 */
+				IVideoPicture picture = IVideoPicture.make(
+						videoCoder.getPixelType(), videoCoder.getWidth(),
+						videoCoder.getHeight());
+
+				try {
+					int offset = 0;
+					while (offset < packet.getSize()) {
+						// System.out.println("VideoManager.decode(): decode one image");
+						/*
+						 * Now, we decode the video, checking for any errors.
+						 */
+						int bytesDecoded = videoCoder.decodeVideo(picture,
+								packet, offset);
+						if (bytesDecoded < 0)
+							throw new RuntimeException(
+									"got error decoding video");
+						offset += bytesDecoded;
+
+						/*
+						 * Some decoders will consume data in a packet, but will
+						 * not be able to construct a full video picture yet.
+						 * Therefore you should always check if you got a
+						 * complete picture from the decoder
+						 */
+						if (picture.isComplete()) {
+							// System.out.println("VideoManager.decode(): image complete");
+							IVideoPicture newPic = picture;
+							/*
+							 * If the resampler is not null, that means we
+							 * didn't get the video in BGR24 format and need to
+							 * convert it into BGR24 format.
+							 */
+							if (resampler != null) {
+								// we must resample
+								newPic = IVideoPicture
+										.make(resampler.getOutputPixelFormat(),
+												picture.getWidth(),
+												picture.getHeight());
+								if (resampler.resample(newPic, picture) < 0)
+									throw new RuntimeException(
+											"could not resample video");
+							}
+							if (newPic.getPixelType() != IPixelFormat.Type.BGR24)
+								throw new RuntimeException(
+										"could not decode video as BGR 24 bit data");
+
+							/**
+							 * We could just display the images as quickly as we
+							 * decode them, but it turns out we can decode a lot
+							 * faster than you think.
+							 * 
+							 * So instead, the following code does a poor-man's
+							 * version of trying to match up the frame-rate
+							 * requested for each IVideoPicture with the system
+							 * clock time on your computer.
+							 * 
+							 * Remember that all Xuggler IAudioSamples and
+							 * IVideoPicture objects always give timestamps in
+							 * Microseconds, relative to the first decoded item.
+							 * If instead you used the packet timestamps, they
+							 * can be in different units depending on your
+							 * IContainer, and IStream and things can get hairy
+							 * quickly.
+							 */
+							if (firstTimestampInStream == Global.NO_PTS) {
+								// This is our first time through
+								firstTimestampInStream = picture.getTimeStamp();
+								// get the starting clock time so we can hold up
+								// frames until the right time.
+								systemClockStartTime = System
+										.currentTimeMillis();
+							} else {
+								long systemClockCurrentTime = System
+										.currentTimeMillis();
+								long millisecondsClockTimeSinceStartofVideo = systemClockCurrentTime
+										- systemClockStartTime;
+
+								// compute how long for this frame since the
+								// first frame in the stream.
+								// remember that IVideoPicture and IAudioSamples
+								// timestamps are always in MICROSECONDS,
+								// so we divide by 1000 to get milliseconds.
+								long millisecondsStreamTimeSinceStartOfVideo = (picture
+										.getTimeStamp() - firstTimestampInStream) / 1000;
+								final long millisecondsTolerance = 50; // and we
+																		// give
+																		// ourselfs
+																		// 50 ms
+																		// of
+																		// tolerance
+								final long millisecondsToSleep = (millisecondsStreamTimeSinceStartOfVideo - (millisecondsClockTimeSinceStartofVideo + millisecondsTolerance));
+								if (millisecondsToSleep > 0) {
+									try {
+										Thread.sleep(millisecondsToSleep);
+									} catch (InterruptedException e) {
+										// we might get this when the user
+										// closes the dialog box, so just return
+										// from the method.
+										return;
+									}
+								}
+							}
+
+							// And finally, convert the BGR24 to an Java
+							// buffered image
+							BufferedImage javaImage = Utils
+									.videoPictureToImage(newPic);
+
+							// and display it on the Java Swing window
+							if (listener != null)
+								listener.imageUpdated(javaImage);
+						}
+					} // end of while
+				} catch (Exception exc) {
+					exc.printStackTrace();
+				}
+			} else {
+				/*
+				 * This packet isn't part of our video stream, so we just
+				 * silently drop it.
+				 */
+				do {
+				} while (false);
+			}
+
+		}
+		/*
+		 * Technically since we're exiting anyway, these will be cleaned up by
+		 * the garbage collector... but because we're nice people and want to be
+		 * invited places for Christmas, we're going to show how to clean up.
+		 */
+		if (videoCoder != null) {
+			videoCoder.close();
+			videoCoder = null;
+		}
+		if (container != null) {
+			container.close();
+			container = null;
+		}
+	}
+
+	@Override
+	public boolean connect(int port) {
+		try {
+			socket = new Socket(inetaddr, port);
+			socket.setSoTimeout(3000);
+		} catch (IOException e) {
+			e.printStackTrace();
+			return false;
+		}
+		return true;
+	}
+
+	@Override
+	public void close() {
+		try {
+			socket.close();
+		} catch (IOException e) {
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	protected void ticklePort(int port) {
+		byte[] buf = { 0x01, 0x00, 0x00, 0x00 };
+		try {
+			OutputStream os = socket.getOutputStream();
+			os.write(buf);
+		} catch (IOException e) {
+			e.printStackTrace();
+		}
+	}
+}
\ No newline at end of file

src/com/shigeodayo/ardrone/video/uint.java

+package com.shigeodayo.ardrone.video;
+
+import java.io.ByteArrayInputStream;
+//import java.io.ByteArrayOutputStream;
+import java.io.DataInputStream;
+//import java.io.DataOutputStream;
+import java.nio.ByteBuffer;
+//import java.security.AllPermission;
+
+//Copyright  2007-2011, PARROT SA, all rights reserved. 
+
+//DISCLAIMER 
+//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
+//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
+//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
+//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
+//software, even if advised of the possibility of such damage. 
+
+//Author            : Daniel Schmidt
+//Publishing date   : 2010-01-06 
+//based on work by  : Wilke Jansoone
+
+//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
+//are met:
+//- Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
+//- Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
+//specific prior written permission.
+
+public class uint {
+
+	public String toString() {
+		return Integer.toString(base2, 2);
+	}
+
+	public uint(int base) {
+		this.base2 = base;
+
+	}
+
+	public uint(uint that) {
+		this.base2 = that.base2;
+	}
+
+	public uint(byte[] bp, int start) {
+		try {
+			byte[] b = new byte[4];
+			b[0] = bp[start + 3];
+			b[1] = bp[start + 2];
+			b[2] = bp[start + 1];
+			b[3] = bp[start + 0];
+
+			ByteArrayInputStream bas = new ByteArrayInputStream(b);
+			DataInputStream din = new DataInputStream(bas);
+
+			this.base2 = din.readInt();
+		} catch (Exception e) {
+			throw new RuntimeException("error creating uint", e);
+		}
+	}
+
+	public uint(ByteBuffer bp, int start) {
+		try {
+			ByteBuffer bb = ByteBuffer.allocate(4);
+			bb.put(bp.array()[start + 3]);
+			bb.put(bp.array()[start + 2]);
+			bb.put(bp.array()[start + 1]);
+			bb.put(bp.array()[start + 0]);
+			bb.flip();
+			this.base2 = bb.getInt();
+		} catch (Exception e) {
+			throw new RuntimeException("error creating uint", e);
+		}
+	}
+
+	private int base2;
+
+	public short times(short i) {
+		return (short) (intValue() * i);
+	}
+
+	public uint shiftRight(int i) {
+		// System.out.println("shiftRight[0] " + base2 + " " + i);
+
+		// String str = Integer.toBinaryString(base);
+		int base = base2;
+		// System.out.println("shiftRight[n][1] " + uint.toBinaryString(base));
+
+		base = base >>> i;
+
+		// System.out.println("shiftRight[n][2] " + uint.toBinaryString(base));
+
+		return new uint(base);
+	}
+
+	public uint shiftLeft(int i) {
+		int base = base2;
+		base <<= i;
+
+		return new uint(base);
+		// return Integer.parseInt(base, 2);
+	}
+
+	public int flipBits() {
+		int base = ~base2;
+
+		return base;
+	}
+
+	public int intValue() {
+		return base2;
+
+	}
+
+	public uint and(int andval) {
+		int retval = base2 & andval;
+		return new uint(retval);
+	}
+
+	public void shiftLeftEquals(int i) {
+		int base = base2;
+
+		base <<= i;
+
+		base2 = base;
+	}
+
+	public void shiftRightEquals(int i) {
+		int base = base2;
+
+		base >>>= i;
+
+		base2 = base;
+	}
+
+	public uint or(uint orval) {
+		int retval = base2 | orval.base2;
+		return new uint(retval);
+	}
+
+}

src/exmaples/ARDroneTest.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package exmaples;
+
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.event.InputEvent;
+import java.awt.event.KeyAdapter;
+import java.awt.event.KeyEvent;
+import java.awt.event.WindowAdapter;
+import java.awt.event.WindowEvent;
+import java.awt.image.BufferedImage;
+
+import javax.swing.JFrame;
+import javax.swing.JPanel;
+import javax.swing.SwingUtilities;
+
+import com.shigeodayo.ardrone.ARDrone;
+import com.shigeodayo.ardrone.navdata.AttitudeListener;
+import com.shigeodayo.ardrone.navdata.BatteryListener;
+import com.shigeodayo.ardrone.navdata.DroneState;
+import com.shigeodayo.ardrone.navdata.StateListener;
+import com.shigeodayo.ardrone.navdata.VelocityListener;
+import com.shigeodayo.ardrone.video.ImageListener;
+
+/**
+ * example for Java application
+ * 
+ * @author shigeo
+ * 
+ */
+public class ARDroneTest extends JFrame {
+
+	private static final long serialVersionUID = 1L;
+
+	private ARDrone ardrone = null;
+
+	private MyPanel myPanel = null;
+
+	public ARDroneTest() {
+		initialize();
+	}
+
+	private void initialize() {
+		ardrone = new ARDrone("192.168.1.1");
+		System.out.println("connect drone controller");
+		ardrone.connect();
+		System.out.println("connect drone navdata");
+		ardrone.connectNav();
+		System.out.println("connect drone video");
+		ardrone.connectVideo();
+		System.out.println("start drone");
+		ardrone.start();
+
+		ardrone.addImageUpdateListener(new ImageListener() {
+			@Override
+			public void imageUpdated(BufferedImage image) {
+				if (myPanel != null) {
+					myPanel.setImage(image);
+					myPanel.repaint();
+				}
+			}
+		});
+
+		ardrone.addAttitudeUpdateListener(new AttitudeListener() {
+			@Override
+			public void attitudeUpdated(float pitch, float roll, float yaw,
+					int altitude) {
+				System.out.println("pitch: " + pitch + ", roll: " + roll
+				 + ", yaw: " + yaw + ", altitude: " + altitude);
+			}
+		});
+
+		ardrone.addBatteryUpdateListener(new BatteryListener() {
+			@Override
+			public void batteryLevelChanged(int percentage) {
+				System.out.println("battery: " + percentage + " %");
+			}
+		});
+
+		ardrone.addStateUpdateListener(new StateListener() {
+			@Override
+			public void stateChanged(DroneState state) {
+				 //System.out.println("state: " + state);
+			}
+		});
+
+		ardrone.addVelocityUpdateListener(new VelocityListener() {
+			@Override
+			public void velocityChanged(float vx, float vy, float vz) {
+				 System.out.println("vx: " + vx + ", vy: " + vy + ", vz: " +
+				 vz);
+			}
+		});
+
+		addKeyListener(new KeyAdapter() {
+			public void keyReleased(KeyEvent e) {
+				ardrone.stop();
+			}
+
+			public void keyPressed(KeyEvent e) {
+				int key = e.getKeyCode();
+				int mod = e.getModifiersEx();
+
+				boolean shiftflag = false;
+				if ((mod & InputEvent.SHIFT_DOWN_MASK) != 0) {
+					shiftflag = true;
+				}
+
+				switch (key) {
+				case KeyEvent.VK_ENTER:
+					ardrone.takeOff();
+					break;
+				case KeyEvent.VK_SPACE:
+					ardrone.landing();
+					break;
+				case KeyEvent.VK_S:
+					ardrone.stop();
+					break;
+				case KeyEvent.VK_LEFT:
+					if (shiftflag)
+						ardrone.spinLeft();
+					else 
+						ardrone.goLeft();
+					break;
+				case KeyEvent.VK_RIGHT:
+					if (shiftflag)
+						ardrone.spinRight();
+					else
+						ardrone.goRight();
+					break;
+				case KeyEvent.VK_UP:
+					if (shiftflag)
+						ardrone.up();
+					else
+						ardrone.forward();
+					break;
+				case KeyEvent.VK_DOWN:
+					if (shiftflag)
+						ardrone.down();
+					else
+						ardrone.backward();
+					break;
+				case KeyEvent.VK_1:
+					ardrone.setHorizontalCamera();
+					// System.out.println("1");
+					break;
+				case KeyEvent.VK_2:
+					ardrone.setHorizontalCameraWithVertical();
+					// System.out.println("2");
+					break;
+				case KeyEvent.VK_3:
+					ardrone.setVerticalCamera();
+					// System.out.println("3");
+					break;
+				case KeyEvent.VK_4:
+					ardrone.setVerticalCameraWithHorizontal();
+					// System.out.println("4");
+					break;
+				case KeyEvent.VK_5:
+					ardrone.toggleCamera();
+					// System.out.println("5");
+					break;
+				case KeyEvent.VK_R:
+					ardrone.spinRight();
+					break;
+				case KeyEvent.VK_L:
+					ardrone.spinLeft();
+					break;
+				case KeyEvent.VK_U:
+					ardrone.up();
+					break;
+				case KeyEvent.VK_D:
+					ardrone.down();
+					break;
+				case KeyEvent.VK_E:
+					ardrone.reset();
+					break;
+				}
+			}
+		});
+
+		this.setTitle("ardrone");
+		this.setSize(400, 400);
+		this.add(getMyPanel());
+	}
+
+	private JPanel getMyPanel() {
+		if (myPanel == null) {
+			myPanel = new MyPanel();
+		}
+		return myPanel;
+	}
+
+	/**
+	 * 
+	 * @author shigeo
+	 * 
+	 */
+	private class MyPanel extends JPanel {
+		private static final long serialVersionUID = -7635284252404123776L;
+
+		/** ardrone video image */
+		private BufferedImage image = null;
+
+		public void setImage(BufferedImage image) {
+			this.image = image;
+		}
+
+		public void paint(Graphics g) {
+			g.setColor(Color.white);
+			g.fillRect(0, 0, this.getWidth(), this.getHeight());
+			if (image != null)
+				g.drawImage(image, 0, 0, image.getWidth(), image.getHeight(),
+						null);
+		}
+	}
+
+	public static void main(String args[]) {
+		SwingUtilities.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				final ARDroneTest thisClass = new ARDroneTest();
+				thisClass.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+				thisClass.addWindowListener(new WindowAdapter() {
+					@Override
+					public void windowOpened(WindowEvent e) {
+						// System.out.println("WindowOpened");
+					}
+
+					@Override
+					public void windowClosing(WindowEvent e) {
+						thisClass.dispose();
+					}
+				});
+				thisClass.setVisible(true);
+			}
+		});
+	}
+}
\ No newline at end of file

src/exmaples/FtpClientExample.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package exmaples;
+
+import org.apache.commons.net.ftp.FTP;
+import org.apache.commons.net.ftp.FTPClient;
+
+import com.shigeodayo.ardrone.utils.ARDroneConstants;
+
+import java.io.IOException;
+import java.io.FileOutputStream;
+ 
+public class FtpClientExample {
+	
+    public static void main(String[] args) {
+    	
+        FTPClient client = new FTPClient();
+        FileOutputStream fos = null;
+ 
+        try {
+        	client.connect(ARDroneConstants.IP_ADDRESS, ARDroneConstants.FTP_PORT);
+
+        	if (!client.login("anonymous", ""))
+        		System.err.println("login failed");
+ 
+        	client.setFileType(FTP.BINARY_FILE_TYPE);
+
+        	String filename = "version.txt";
+            fos = new FileOutputStream(filename);
+ 
+            if (!client.retrieveFile("/" + filename, fos))
+            	System.err.println("cannot find file");
+            
+            System.out.println("done");
+        } catch (IOException e) {
+            e.printStackTrace();
+        } finally {
+            try {
+                if (fos != null) {
+                	fos.flush();
+                    fos.close();
+                }
+                client.disconnect();
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+ 
+    }
+}
\ No newline at end of file

src/exmaples/Move3DTest.java

+/**
+   ARDroneForP5
+   https://github.com/shigeodayo/ARDroneForP5
+   Copyright (C) 2013, Shigeo YOSHIDA.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+package exmaples;
+
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.event.InputEvent;
+import java.awt.event.KeyAdapter;
+import java.awt.event.KeyEvent;
+import java.awt.event.WindowAdapter;
+import java.awt.event.WindowEvent;
+import java.awt.image.BufferedImage;
+
+import javax.swing.JFrame;
+import javax.swing.JPanel;
+import javax.swing.SwingUtilities;
+
+import com.shigeodayo.ardrone.ARDrone;
+import com.shigeodayo.ardrone.navdata.AttitudeListener;
+import com.shigeodayo.ardrone.navdata.BatteryListener;
+import com.shigeodayo.ardrone.navdata.DroneState;
+import com.shigeodayo.ardrone.navdata.StateListener;
+import com.shigeodayo.ardrone.navdata.VelocityListener;
+import com.shigeodayo.ardrone.video.ImageListener;
+
+public class Move3DTest extends JFrame {
+	private static final long serialVersionUID = 1L;
+
+	private ARDrone ardrone = null;
+
+	private MyPanel myPanel = null;
+
+	public Move3DTest() {
+		initialize();
+	}
+
+	private void initialize() {
+		ardrone = new ARDrone("192.168.1.1");
+		System.out.println("connect drone controller");
+		ardrone.connect();
+		System.out.println("connect drone navdata");
+		ardrone.connectNav();
+		System.out.println("connect drone video");
+		ardrone.connectVideo();
+		System.out.println("start drone");
+		ardrone.start();
+
+		ardrone.addImageUpdateListener(new ImageListener() {
+			@Override
+			public void imageUpdated(BufferedImage image) {
+				if (myPanel != null) {
+					myPanel.setImage(image);
+					myPanel.repaint();
+				}
+			}
+		});
+
+		ardrone.addAttitudeUpdateListener(new AttitudeListener() {
+			@Override
+			public void attitudeUpdated(float pitch, float roll, float yaw,
+					int altitude) {
+				System.out.println("pitch: " + pitch + ", roll: " + roll
+						+ ", yaw: " + yaw + ", altitude: " + altitude);
+			}
+		});
+
+		ardrone.addBatteryUpdateListener(new BatteryListener() {
+			@Override
+			public void batteryLevelChanged(int percentage) {
+				System.out.println("battery: " + percentage + " %");
+			}
+		});
+
+		ardrone.addStateUpdateListener(new StateListener() {
+			@Override
+			public void stateChanged(DroneState state) {
+				System.out.println("state: " + state);
+			}
+		});
+
+		ardrone.addVelocityUpdateListener(new VelocityListener() {
+			@Override
+			public void velocityChanged(float vx, float vy, float vz) {
+				System.out.println("vx: " + vx + ", vy: " + vy + ", vz: " + vz);
+			}
+		});
+
+		addKeyListener(new KeyAdapter() {
+			public void keyReleased(KeyEvent e) {
+				ardrone.stop();
+			}
+
+			public void keyPressed(KeyEvent e) {
+				int key = e.getKeyCode();
+				int mod = e.getModifiersEx();
+
+				boolean shiftflag = false;
+				if ((mod & InputEvent.SHIFT_DOWN_MASK) != 0) {
+					shiftflag = true;
+				}
+
+				switch (key) {
+				case KeyEvent.VK_ENTER:
+					ardrone.takeOff();
+					break;
+				case KeyEvent.VK_SPACE:
+					ardrone.landing();
+					break;
+				case KeyEvent.VK_S:
+					ardrone.stop();
+					break;
+				case KeyEvent.VK_LEFT:
+					if (shiftflag) {
+						// ardrone.spinLeft();
+						ardrone.move3D(0, 0, 0, 10);
+						shiftflag = false;
+					} else
+						ardrone.move3D(0, 10, 0, 0);
+					// ardrone.goLeft();
+					break;
+				case KeyEvent.VK_RIGHT:
+					if (shiftflag) {
+						// ardrone.spinRight();
+						ardrone.move3D(0, 0, 0, -10);
+						shiftflag = false;
+					} else
+						ardrone.move3D(0, -10, 0, 0);
+					// ardrone.goRight();
+					break;
+				case KeyEvent.VK_UP:
+					if (shiftflag) {
+						// ardrone.up();
+						ardrone.move3D(0, 0, -10, 0);
+						shiftflag = false;
+					} else
+						ardrone.move3D(10, 0, 0, 0);
+					// ardrone.forward();
+					break;
+				case KeyEvent.VK_DOWN:
+					if (shiftflag) {
+						// ardrone.down();
+						ardrone.move3D(0, 0, 10, 0);
+						shiftflag = false;
+					} else
+						ardrone.move3D(-10, 0, 0, 0);
+					// ardrone.backward();
+					break;
+				case KeyEvent.VK_R:
+					// ardrone.spinRight();
+					ardrone.move3D(0, 0, 0, 10);
+					break;
+				case KeyEvent.VK_L:
+					// ardrone.spinLeft();
+					ardrone.move3D(0, 0, 0, -10);
+					break;
+				case KeyEvent.VK_U:
+					// ardrone.up();
+					ardrone.move3D(0, 0, -10, 0);
+					break;
+				case KeyEvent.VK_D:
+					// ardrone.down();
+					ardrone.move3D(0, 0, 10, 0);
+					break;
+				case KeyEvent.VK_E:
+					ardrone.reset();
+					break;
+				case KeyEvent.VK_Z:
+					ardrone.move3D(10, 10, 10, 10);
+					break;
+				case KeyEvent.VK_X:
+					ardrone.move3D(-10, -10, -10, -10);
+					break;
+				case KeyEvent.VK_C:
+					ardrone.move3D(10, 10, 0, 0);
+					break;
+				case KeyEvent.VK_V:
+					ardrone.move3D(-10, -10, 0, 0);
+					break;
+				}
+			}
+		});
+
+		this.setTitle("ardrone");
+		this.setSize(400, 400);
+		this.add(getMyPanel());
+	}
+
+	private JPanel getMyPanel() {
+		if (myPanel == null) {
+			myPanel = new MyPanel();
+		}
+		return myPanel;
+	}
+
+	private class MyPanel extends JPanel {
+		private static final long serialVersionUID = -7635284252404123776L;
+
+		/** ardrone video image */
+		private BufferedImage image = null;
+
+		public void setImage(BufferedImage image) {
+			this.image = image;
+		}
+
+		public void paint(Graphics g) {
+			g.setColor(Color.white);
+			g.fillRect(0, 0, this.getWidth(), this.getHeight());
+			if (image != null)
+				g.drawImage(image, 0, 0, image.getWidth(), image.getHeight(),
+						null);
+		}
+	}
+
+	public static void main(String args[]) {
+		SwingUtilities.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				final Move3DTest thisClass = new Move3DTest();
+				thisClass.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+				thisClass.addWindowListener(new WindowAdapter() {
+					@Override
+					public void windowOpened(WindowEvent e) {
+						System.out.println("WindowOpened");
+					}
+
+					@Override
+					public void windowClosing(WindowEvent e) {
+						thisClass.dispose();
+					}
+				});
+				thisClass.setVisible(true);
+			}
+		});
+	}
+}