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)

2013-01-28 17:42:03 +02:00
commit 521fc574d5
@@ -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;
+    }
    
 }
@@ -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;
@@ -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 ();
+            }
+        }         
+    }
+
+}
@@ -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);
        }