From 521fc574d5f06972710c7148f08a096e08abfbf0 Mon Sep 17 00:00:00 2001 From: Denis Shmyga Date: Mon, 28 Jan 2013 17:42:03 +0200 Subject: [PATCH] Added API method to detect ARDrone version. Corrected Drone initialization code: ARDrone 2.0 has TCP video port while ARDrone 1.0 has UDP . Now library will automatically choose correct reader and decoder. For now ARDrone 2.0 video is disabled (we still working on pure video decoder) --- .../java/com/codeminders/ardrone/ARDrone.java | 68 ++++++++++++++++--- .../ardrone/data/reader/TCPDataRader.java | 2 +- .../version/ftp/DroneFTPversionReader.java | 59 ++++++++++++++++ .../ardrone/decoder/TestH264DataDecoder.java | 20 +++--- 4 files changed, 128 insertions(+), 21 deletions(-) create mode 100644 javadrone-api/src/main/java/com/codeminders/ardrone/version/ftp/DroneFTPversionReader.java diff --git a/javadrone-api/src/main/java/com/codeminders/ardrone/ARDrone.java b/javadrone-api/src/main/java/com/codeminders/ardrone/ARDrone.java index e16aefb..6473990 100644 --- a/javadrone-api/src/main/java/com/codeminders/ardrone/ARDrone.java +++ b/javadrone-api/src/main/java/com/codeminders/ardrone/ARDrone.java @@ -14,12 +14,15 @@ import com.codeminders.ardrone.data.ChannelProcessor; import com.codeminders.ardrone.data.navdata.FlyingState; import com.codeminders.ardrone.data.navdata.Mode; import com.codeminders.ardrone.data.reader.LigthUDPDataReader; +import com.codeminders.ardrone.data.reader.TCPDataRader; import com.codeminders.ardrone.data.reader.UDPDataReader; import com.codeminders.ardrone.commands.*; import com.codeminders.ardrone.data.decoder.ardrone10.ARDrone10NavDataDecoder; import com.codeminders.ardrone.data.decoder.ardrone10.ARDrone10VideoDataDecoder; import com.codeminders.ardrone.data.logger.ARDroneDataReaderAndLogWrapper; import com.codeminders.ardrone.data.logger.DataLogger; +import com.codeminders.ardrone.version.DroneVersionReader; +import com.codeminders.ardrone.version.ftp.DroneFTPversionReader; public class ARDrone { @@ -111,14 +114,13 @@ public class ARDrone private static final int VIDEO_PORT = 5555; private static final int NAVDATA_BUFFER_SIZE = 4096; private static final int VIDEO_BUFFER_SIZE = 100 * 1024; - - // private static final int CONTROL_PORT = 5559; final static byte[] DEFAULT_DRONE_IP = { (byte) 192, (byte) 168, (byte) 1, (byte) 1 }; + private static final int DEFAULT_DRONE_VERSION = 1; + private InetAddress drone_addr; private DatagramSocket cmd_socket; - // private Socket control_socket; private CommandQueue cmd_queue = new CommandQueue(CMD_QUEUE_SIZE); @@ -144,6 +146,8 @@ public class ARDrone private VideoDataDecoder ext_video_data_decoder; private NavDataDecoder ext_nav_data_decoder; + + private DroneVersionReader versionReader; public ARDrone() throws UnknownHostException { @@ -155,6 +159,8 @@ public class ARDrone this.drone_addr = drone_addr; this.navDataReconnectTimeout = navDataReconnectTimeout; this.videoReconnectTimeout = videoReconnectTimeout; + + this.versionReader = new DroneFTPversionReader(drone_addr); } public void addImageListener(DroneVideoListener l) @@ -295,11 +301,20 @@ public class ARDrone { connect(null, null); } - + public void connect(DataLogger videoLogger, DataLogger navdataLogger) throws IOException { try { + int version = DEFAULT_DRONE_VERSION; + try { + String versionStr = versionReader.readDroneVersion(); + log.log(Level.FINER, "Drone version string: " + versionStr); + version = Integer.parseInt(versionStr.substring(0, versionStr.indexOf('.'))); + } catch (NumberFormatException e) { + log.log(Level.SEVERE, "Failed to discover drone version. Using configuration for drone version: " + version, e); + } + cmd_socket = new DatagramSocket(); cmd_sender = new CommandSender(cmd_queue, this, drone_addr, cmd_socket); @@ -310,7 +325,6 @@ public class ARDrone enableVideo(); enableAutomaticVideoBitrate(); - NavDataDecoder nav_data_decoder = (null == ext_nav_data_decoder) ? new ARDrone10NavDataDecoder(this, NAVDATA_BUFFER_SIZE) : @@ -324,15 +338,17 @@ public class ARDrone drone_nav_channel_processor = new ChannelProcessor(nav_data_reader, nav_data_decoder); VideoDataDecoder video_data_decoder = (null == ext_video_data_decoder) ? - new ARDrone10VideoDataDecoder(this, VIDEO_BUFFER_SIZE) + getVideoDecoder(version) : ext_video_data_decoder; ARDroneDataReader video_data_reader = (null == videoLogger) ? - new UDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout) + getVideoReader(version) : new ARDroneDataReaderAndLogWrapper(new UDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout), videoLogger); - drone_video_channel_processor = new ChannelProcessor(video_data_reader, video_data_decoder); + if (null != video_data_reader && null != video_data_decoder) { + drone_video_channel_processor = new ChannelProcessor(video_data_reader, video_data_decoder); + } changeState(State.CONNECTING); @@ -343,6 +359,29 @@ public class ARDrone } } + private VideoDataDecoder getVideoDecoder(int version) throws IOException { + switch (version) { + case 1: + return new ARDrone10VideoDataDecoder(this, VIDEO_BUFFER_SIZE); + case 2: + return null; // no decoder implemented yet + default: + return new ARDrone10VideoDataDecoder(this, VIDEO_BUFFER_SIZE); + } + + } + + private ARDroneDataReader getVideoReader(int version) throws IOException { + switch (version) { + case 1: + return new LigthUDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout); + case 2: + return new TCPDataRader(drone_addr, VIDEO_PORT, videoReconnectTimeout); + default: + return new LigthUDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout); + } + } + public void disableAutomaticVideoBitrate() throws IOException { cmd_queue.add(new ConfigureCommand("video:bitrate_control_mode", "0")); @@ -724,6 +763,17 @@ public class ARDrone public void setExternalVideoDataDecoder(NavDataDecoder ext_nav_data_decoder) { this.ext_nav_data_decoder = ext_nav_data_decoder; } - + /** + * Read Drone version. + * @return Drone version string e.g. "1.10.10". null - if version can't be obtained + */ + public String getDroneVersion() { + try { + return versionReader.readDroneVersion(); + } catch (IOException e) { + log.log(Level.SEVERE, "Failed to read drone version.", e); + } + return null; + } } diff --git a/javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java b/javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java index ff23501..741408e 100644 --- a/javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java +++ b/javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java @@ -22,7 +22,7 @@ public class TCPDataRader implements ARDroneDataReader { private InputStream socketInput; - public TCPDataRader(InetAddress drone_addr, int data_port, int buffer_size, int timeout) throws IOException { + public TCPDataRader(InetAddress drone_addr, int data_port, int timeout) throws IOException { super(); this.drone_addr = drone_addr; diff --git a/javadrone-api/src/main/java/com/codeminders/ardrone/version/ftp/DroneFTPversionReader.java b/javadrone-api/src/main/java/com/codeminders/ardrone/version/ftp/DroneFTPversionReader.java new file mode 100644 index 0000000..a1e23b0 --- /dev/null +++ b/javadrone-api/src/main/java/com/codeminders/ardrone/version/ftp/DroneFTPversionReader.java @@ -0,0 +1,59 @@ +package com.codeminders.ardrone.version.ftp; + +import java.io.ByteArrayOutputStream; +import java.io.IOException; +import java.io.InputStream; +import java.net.InetAddress; +import java.net.URL; +import java.net.URLConnection; +import java.util.logging.Logger; + +import com.codeminders.ardrone.version.DroneVersionReader; + +public class DroneFTPversionReader implements DroneVersionReader { + + private Logger log = Logger.getLogger(getClass().getName()); + + private static final int FTP_PORT = 5551; + private static final String VERSION_FILE_NAME = "version.txt"; + + String ftpVersionFileLocation; + + public DroneFTPversionReader(InetAddress drone_addr) { + this.ftpVersionFileLocation = "ftp://"+drone_addr.getHostAddress() + ":" + FTP_PORT + "/" + VERSION_FILE_NAME; + } + + @Override + public String readDroneVersion() throws IOException { + + InputStream is = null; + ByteArrayOutputStream bos = null; + try { + log.info("Attempting to read AR Drone version using FTP. Version file is: "+ ftpVersionFileLocation); + URL url = new URL(ftpVersionFileLocation); + URLConnection ftpConnection = url.openConnection(); + log.info(ftpVersionFileLocation + "- Connection Opened"); + + is = ftpConnection.getInputStream(); + bos = new ByteArrayOutputStream(); + + byte[] buffer = new byte[1024]; + int readCount; + + while((readCount = is.read(buffer)) > 0) + { + bos.write(buffer, 0, readCount); + } + + return bos.toString(); + } finally { + if (null != bos) { + bos.close(); + } + if (null != is) { + is.close (); + } + } + } + +} diff --git a/javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/TestH264DataDecoder.java b/javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/TestH264DataDecoder.java index 6b9da86..4b840f3 100644 --- a/javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/TestH264DataDecoder.java +++ b/javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/TestH264DataDecoder.java @@ -2,6 +2,8 @@ package com.codeminders.ardrone.decoder; import java.io.InputStream; import java.util.Arrays; +import java.util.logging.Level; +import java.util.logging.Logger; import com.codeminders.ardrone.ARDrone; import com.codeminders.ardrone.VideoDataDecoder; @@ -13,10 +15,9 @@ import com.twilight.h264.player.FrameUtils; public class TestH264DataDecoder extends VideoDataDecoder { - ARDrone drone; + Logger log = Logger.getLogger(this.getClass().getName()); - int buffer_size; - public int INBUF_SIZE; + public static final int INBUF_SIZE = 65535; H264Decoder codec; MpegEncContext c = null; @@ -35,12 +36,9 @@ public class TestH264DataDecoder extends VideoDataDecoder { int dataPointer; - public TestH264DataDecoder(ARDrone drone, int buffer_size) { + public TestH264DataDecoder(ARDrone drone) { super(drone); - this.drone = drone; - this.buffer_size = buffer_size; - INBUF_SIZE = buffer_size; - + avpkt = new AVPacket(); avpkt.av_init_packet(); @@ -143,14 +141,14 @@ public class TestH264DataDecoder extends VideoDataDecoder { } } catch(Exception ie) { // Any exception, we should try to proceed reading next packet! - ie.printStackTrace(); + log.log(Level.FINEST, "Error decodeing frame", ie); } // try } // while - } catch(Exception e) { - e.printStackTrace(); + } catch(Exception ex) { + log.log(Level.FINEST, "Error in decoder initialization", ex); }