From e3dd9624c77764cb031cda176f7f17c8fda65b84 Mon Sep 17 00:00:00 2001 From: KazushiKawamura Date: Tue, 6 Nov 2018 13:16:37 +0900 Subject: [PATCH] upload source files --- ARDroneForP5_Sample.pde | 106 ++ src/com/shigeodayo/ardrone/ARDrone.java | 495 ++++++++ .../shigeodayo/ardrone/ARDroneInterface.java | 97 ++ .../ardrone/command/CommandManager.java | 352 ++++++ .../ardrone/command/CommandManager1.java | 98 ++ .../ardrone/command/CommandManager2.java | 164 +++ .../ardrone/manager/AbstractManager.java | 56 + .../ardrone/navdata/AttitudeListener.java | 10 + .../ardrone/navdata/BatteryListener.java | 10 + .../ardrone/navdata/DetectType.java | 26 + .../navdata/DetectionDataListener.java | 26 + .../ardrone/navdata/DroneState.java | 28 + .../ardrone/navdata/NavDataException.java | 15 + .../ardrone/navdata/NavDataManager.java | 92 ++ .../ardrone/navdata/NavDataManager1.java | 40 + .../ardrone/navdata/NavDataManager2.java | 41 + .../ardrone/navdata/NavDataParser.java | 113 ++ .../ardrone/navdata/StateListener.java | 11 + .../ardrone/navdata/VelocityListener.java | 10 + .../ardrone/processing/ARDroneForP5.java | 261 ++++ .../ardrone/utils/ARDroneConstants.java | 42 + .../shigeodayo/ardrone/utils/ARDroneInfo.java | 122 ++ .../ardrone/utils/ARDroneVersion.java | 22 + .../ardrone/video/BufferedVideoImage.java | 1084 +++++++++++++++++ .../ardrone/video/ImageDataException.java | 27 + .../ardrone/video/ImageListener.java | 24 + .../shigeodayo/ardrone/video/ImageSlice.java | 32 + .../shigeodayo/ardrone/video/MacroBlock.java | 47 + .../ardrone/video/PictureFormats.java | 31 + .../ardrone/video/ReadRawFileImage.java | 99 ++ .../shigeodayo/ardrone/video/VideoImage.java | 1082 ++++++++++++++++ .../ardrone/video/VideoManager.java | 51 + .../ardrone/video/VideoManager1.java | 65 + .../ardrone/video/VideoManager2.java | 311 +++++ src/com/shigeodayo/ardrone/video/uint.java | 140 +++ src/exmaples/ARDroneTest.java | 255 ++++ src/exmaples/FtpClientExample.java | 65 + src/exmaples/Move3DTest.java | 250 ++++ 38 files changed, 5800 insertions(+) create mode 100644 ARDroneForP5_Sample.pde create mode 100644 src/com/shigeodayo/ardrone/ARDrone.java create mode 100644 src/com/shigeodayo/ardrone/ARDroneInterface.java create mode 100644 src/com/shigeodayo/ardrone/command/CommandManager.java create mode 100644 src/com/shigeodayo/ardrone/command/CommandManager1.java create mode 100644 src/com/shigeodayo/ardrone/command/CommandManager2.java create mode 100644 src/com/shigeodayo/ardrone/manager/AbstractManager.java create mode 100644 src/com/shigeodayo/ardrone/navdata/AttitudeListener.java create mode 100644 src/com/shigeodayo/ardrone/navdata/BatteryListener.java create mode 100644 src/com/shigeodayo/ardrone/navdata/DetectType.java create mode 100644 src/com/shigeodayo/ardrone/navdata/DetectionDataListener.java create mode 100644 src/com/shigeodayo/ardrone/navdata/DroneState.java create mode 100644 src/com/shigeodayo/ardrone/navdata/NavDataException.java create mode 100644 src/com/shigeodayo/ardrone/navdata/NavDataManager.java create mode 100644 src/com/shigeodayo/ardrone/navdata/NavDataManager1.java create mode 100644 src/com/shigeodayo/ardrone/navdata/NavDataManager2.java create mode 100644 src/com/shigeodayo/ardrone/navdata/NavDataParser.java create mode 100644 src/com/shigeodayo/ardrone/navdata/StateListener.java create mode 100644 src/com/shigeodayo/ardrone/navdata/VelocityListener.java create mode 100644 src/com/shigeodayo/ardrone/processing/ARDroneForP5.java create mode 100644 src/com/shigeodayo/ardrone/utils/ARDroneConstants.java create mode 100644 src/com/shigeodayo/ardrone/utils/ARDroneInfo.java create mode 100644 src/com/shigeodayo/ardrone/utils/ARDroneVersion.java create mode 100644 src/com/shigeodayo/ardrone/video/BufferedVideoImage.java create mode 100644 src/com/shigeodayo/ardrone/video/ImageDataException.java create mode 100644 src/com/shigeodayo/ardrone/video/ImageListener.java create mode 100644 src/com/shigeodayo/ardrone/video/ImageSlice.java create mode 100644 src/com/shigeodayo/ardrone/video/MacroBlock.java create mode 100644 src/com/shigeodayo/ardrone/video/PictureFormats.java create mode 100644 src/com/shigeodayo/ardrone/video/ReadRawFileImage.java create mode 100644 src/com/shigeodayo/ardrone/video/VideoImage.java create mode 100644 src/com/shigeodayo/ardrone/video/VideoManager.java create mode 100644 src/com/shigeodayo/ardrone/video/VideoManager1.java create mode 100644 src/com/shigeodayo/ardrone/video/VideoManager2.java create mode 100644 src/com/shigeodayo/ardrone/video/uint.java create mode 100644 src/exmaples/ARDroneTest.java create mode 100644 src/exmaples/FtpClientExample.java create mode 100644 src/exmaples/Move3DTest.java diff --git a/ARDroneForP5_Sample.pde b/ARDroneForP5_Sample.pde new file mode 100644 index 0000000..5ced5d0 --- /dev/null +++ b/ARDroneForP5_Sample.pde @@ -0,0 +1,106 @@ +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 + } + } +} + diff --git a/src/com/shigeodayo/ardrone/ARDrone.java b/src/com/shigeodayo/ardrone/ARDrone.java new file mode 100644 index 0000000..35be34a --- /dev/null +++ b/src/com/shigeodayo/ardrone/ARDrone.java @@ -0,0 +1,495 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/ARDroneInterface.java b/src/com/shigeodayo/ardrone/ARDroneInterface.java new file mode 100644 index 0000000..a15c414 --- /dev/null +++ b/src/com/shigeodayo/ardrone/ARDroneInterface.java @@ -0,0 +1,97 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/command/CommandManager.java b/src/com/shigeodayo/ardrone/command/CommandManager.java new file mode 100644 index 0000000..0413873 --- /dev/null +++ b/src/com/shigeodayo/ardrone/command/CommandManager.java @@ -0,0 +1,352 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/command/CommandManager1.java b/src/com/shigeodayo/ardrone/command/CommandManager1.java new file mode 100644 index 0000000..078cdb8 --- /dev/null +++ b/src/com/shigeodayo/ardrone/command/CommandManager1.java @@ -0,0 +1,98 @@ +/** + 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 !!"); + } +} diff --git a/src/com/shigeodayo/ardrone/command/CommandManager2.java b/src/com/shigeodayo/ardrone/command/CommandManager2.java new file mode 100644 index 0000000..b881e05 --- /dev/null +++ b/src/com/shigeodayo/ardrone/command/CommandManager2.java @@ -0,0 +1,164 @@ +/** + 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(); + } + +} diff --git a/src/com/shigeodayo/ardrone/manager/AbstractManager.java b/src/com/shigeodayo/ardrone/manager/AbstractManager.java new file mode 100644 index 0000000..4522a5c --- /dev/null +++ b/src/com/shigeodayo/ardrone/manager/AbstractManager.java @@ -0,0 +1,56 @@ +/** + 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(); + } + } +} diff --git a/src/com/shigeodayo/ardrone/navdata/AttitudeListener.java b/src/com/shigeodayo/ardrone/navdata/AttitudeListener.java new file mode 100644 index 0000000..c47f97f --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/AttitudeListener.java @@ -0,0 +1,10 @@ +/* + * 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); +} diff --git a/src/com/shigeodayo/ardrone/navdata/BatteryListener.java b/src/com/shigeodayo/ardrone/navdata/BatteryListener.java new file mode 100644 index 0000000..91731b1 --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/BatteryListener.java @@ -0,0 +1,10 @@ +/* + * 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); +} diff --git a/src/com/shigeodayo/ardrone/navdata/DetectType.java b/src/com/shigeodayo/ardrone/navdata/DetectType.java new file mode 100644 index 0000000..10b2e40 --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/DetectType.java @@ -0,0 +1,26 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/navdata/DetectionDataListener.java b/src/com/shigeodayo/ardrone/navdata/DetectionDataListener.java new file mode 100644 index 0000000..debf9fd --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/DetectionDataListener.java @@ -0,0 +1,26 @@ +/** + 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); +} diff --git a/src/com/shigeodayo/ardrone/navdata/DroneState.java b/src/com/shigeodayo/ardrone/navdata/DroneState.java new file mode 100644 index 0000000..687b8da --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/DroneState.java @@ -0,0 +1,28 @@ +/* + * 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 diff --git a/src/com/shigeodayo/ardrone/navdata/NavDataException.java b/src/com/shigeodayo/ardrone/navdata/NavDataException.java new file mode 100644 index 0000000..326cac8 --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/NavDataException.java @@ -0,0 +1,15 @@ +/* + * 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); + } +} diff --git a/src/com/shigeodayo/ardrone/navdata/NavDataManager.java b/src/com/shigeodayo/ardrone/navdata/NavDataManager.java new file mode 100644 index 0000000..0a10fee --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/NavDataManager.java @@ -0,0 +1,92 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/navdata/NavDataManager1.java b/src/com/shigeodayo/ardrone/navdata/NavDataManager1.java new file mode 100644 index 0000000..aec8f4b --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/NavDataManager1.java @@ -0,0 +1,40 @@ +/** + 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(); + } +} diff --git a/src/com/shigeodayo/ardrone/navdata/NavDataManager2.java b/src/com/shigeodayo/ardrone/navdata/NavDataManager2.java new file mode 100644 index 0000000..4741350 --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/NavDataManager2.java @@ -0,0 +1,41 @@ +/** + 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(); + } +} diff --git a/src/com/shigeodayo/ardrone/navdata/NavDataParser.java b/src/com/shigeodayo/ardrone/navdata/NavDataParser.java new file mode 100644 index 0000000..40473fc --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/NavDataParser.java @@ -0,0 +1,113 @@ +/* + * 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 diff --git a/src/com/shigeodayo/ardrone/navdata/StateListener.java b/src/com/shigeodayo/ardrone/navdata/StateListener.java new file mode 100644 index 0000000..721c8fd --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/StateListener.java @@ -0,0 +1,11 @@ +/* + * 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); +} diff --git a/src/com/shigeodayo/ardrone/navdata/VelocityListener.java b/src/com/shigeodayo/ardrone/navdata/VelocityListener.java new file mode 100644 index 0000000..a5f06dc --- /dev/null +++ b/src/com/shigeodayo/ardrone/navdata/VelocityListener.java @@ -0,0 +1,10 @@ +/* + * 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); +} diff --git a/src/com/shigeodayo/ardrone/processing/ARDroneForP5.java b/src/com/shigeodayo/ardrone/processing/ARDroneForP5.java new file mode 100644 index 0000000..1d447fe --- /dev/null +++ b/src/com/shigeodayo/ardrone/processing/ARDroneForP5.java @@ -0,0 +1,261 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/utils/ARDroneConstants.java b/src/com/shigeodayo/ardrone/utils/ARDroneConstants.java new file mode 100644 index 0000000..ece0f61 --- /dev/null +++ b/src/com/shigeodayo/ardrone/utils/ARDroneConstants.java @@ -0,0 +1,42 @@ +/** + 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 + +} diff --git a/src/com/shigeodayo/ardrone/utils/ARDroneInfo.java b/src/com/shigeodayo/ardrone/utils/ARDroneInfo.java new file mode 100644 index 0000000..c85f474 --- /dev/null +++ b/src/com/shigeodayo/ardrone/utils/ARDroneInfo.java @@ -0,0 +1,122 @@ +/** + 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; + } +} diff --git a/src/com/shigeodayo/ardrone/utils/ARDroneVersion.java b/src/com/shigeodayo/ardrone/utils/ARDroneVersion.java new file mode 100644 index 0000000..2d819a5 --- /dev/null +++ b/src/com/shigeodayo/ardrone/utils/ARDroneVersion.java @@ -0,0 +1,22 @@ +/** + 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 +} diff --git a/src/com/shigeodayo/ardrone/video/BufferedVideoImage.java b/src/com/shigeodayo/ardrone/video/BufferedVideoImage.java new file mode 100644 index 0000000..9e0b407 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/BufferedVideoImage.java @@ -0,0 +1,1084 @@ +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 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; + + // / + // / Length of one row of pixels in the destination image in bytes. + // / + 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 diff --git a/src/com/shigeodayo/ardrone/video/ImageDataException.java b/src/com/shigeodayo/ardrone/video/ImageDataException.java new file mode 100644 index 0000000..6b8c0b7 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/ImageDataException.java @@ -0,0 +1,27 @@ +/** + 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); + } +} diff --git a/src/com/shigeodayo/ardrone/video/ImageListener.java b/src/com/shigeodayo/ardrone/video/ImageListener.java new file mode 100644 index 0000000..8142fa7 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/ImageListener.java @@ -0,0 +1,24 @@ +/** + 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); +} diff --git a/src/com/shigeodayo/ardrone/video/ImageSlice.java b/src/com/shigeodayo/ardrone/video/ImageSlice.java new file mode 100644 index 0000000..164bee7 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/ImageSlice.java @@ -0,0 +1,32 @@ +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 diff --git a/src/com/shigeodayo/ardrone/video/MacroBlock.java b/src/com/shigeodayo/ardrone/video/MacroBlock.java new file mode 100644 index 0000000..d8bf186 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/MacroBlock.java @@ -0,0 +1,47 @@ +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 diff --git a/src/com/shigeodayo/ardrone/video/PictureFormats.java b/src/com/shigeodayo/ardrone/video/PictureFormats.java new file mode 100644 index 0000000..676d88a --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/PictureFormats.java @@ -0,0 +1,31 @@ +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 { + // / + // / 176px x 144px + // / + public static final int Cif = 1; + // / + // / 320px x 240px + // / + public static final int Vga = 2; +} diff --git a/src/com/shigeodayo/ardrone/video/ReadRawFileImage.java b/src/com/shigeodayo/ardrone/video/ReadRawFileImage.java new file mode 100644 index 0000000..a1ee4ff --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/ReadRawFileImage.java @@ -0,0 +1,99 @@ +//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(); + } +} diff --git a/src/com/shigeodayo/ardrone/video/VideoImage.java b/src/com/shigeodayo/ardrone/video/VideoImage.java new file mode 100644 index 0000000..8bf6d9e --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/VideoImage.java @@ -0,0 +1,1082 @@ +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 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; + + // / + // / Length of one row of pixels in the destination image in bytes. + // / + 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 diff --git a/src/com/shigeodayo/ardrone/video/VideoManager.java b/src/com/shigeodayo/ardrone/video/VideoManager.java new file mode 100644 index 0000000..8e89fcc --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/VideoManager.java @@ -0,0 +1,51 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/video/VideoManager1.java b/src/com/shigeodayo/ardrone/video/VideoManager1.java new file mode 100644 index 0000000..9f39c07 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/VideoManager1.java @@ -0,0 +1,65 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/video/VideoManager2.java b/src/com/shigeodayo/ardrone/video/VideoManager2.java new file mode 100644 index 0000000..417b84b --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/VideoManager2.java @@ -0,0 +1,311 @@ +/** + 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 diff --git a/src/com/shigeodayo/ardrone/video/uint.java b/src/com/shigeodayo/ardrone/video/uint.java new file mode 100644 index 0000000..22c4a33 --- /dev/null +++ b/src/com/shigeodayo/ardrone/video/uint.java @@ -0,0 +1,140 @@ +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); + } + +} diff --git a/src/exmaples/ARDroneTest.java b/src/exmaples/ARDroneTest.java new file mode 100644 index 0000000..1adb23c --- /dev/null +++ b/src/exmaples/ARDroneTest.java @@ -0,0 +1,255 @@ +/** + 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 diff --git a/src/exmaples/FtpClientExample.java b/src/exmaples/FtpClientExample.java new file mode 100644 index 0000000..5c9caa2 --- /dev/null +++ b/src/exmaples/FtpClientExample.java @@ -0,0 +1,65 @@ +/** + 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 diff --git a/src/exmaples/Move3DTest.java b/src/exmaples/Move3DTest.java new file mode 100644 index 0000000..f9b18e5 --- /dev/null +++ b/src/exmaples/Move3DTest.java @@ -0,0 +1,250 @@ +/** + 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); + } + }); + } +} -- 2.22.0