Ardronev2 support

drone/hook/CameraDroneV1.h

@@ -0,0 +1,22 @@
+/*
+
+AR.Pwn Hook Program
+Copyright 2015, Team SNARC / VECLabs
+Defines camera specific date for ArDroneV1
+
+*/
+static const char * const CAM1_DEVICE = "/dev/video0";
+
+static const size_t CAM1_WIDTH = 640;
+static const size_t CAM1_HEIGHT = 480;
+
+static const char * const CAM2_DEVICE = "/dev/video1";
+
+static const size_t CAM2_WIDTH = 176;
+static const size_t CAM2_HEIGHT = 144;
+
+const size_t getBufferSize(size_t width, size_t height)
+{
+    return width * height * 3 / 2;
+}
+

drone/hook/CameraDroneV2.h

@@ -0,0 +1,22 @@
+/*
+
+AR.Pwn Hook Program
+Copyright 2015, Team SNARC / VECLabs
+Defines camera specific date for ArDroneV2
+
+*/
+static const char * const CAM1_DEVICE = "/dev/video1";
+
+static const size_t CAM1_WIDTH = 1280;
+static const size_t CAM1_HEIGHT = 720;
+
+static const char * const CAM2_DEVICE = "/dev/video2";
+
+static const size_t CAM2_WIDTH = 320;
+static const size_t CAM2_HEIGHT = 240;
+
+/** AR Drone v2 uses YUV images with two channel */
+const int getBufferSize(size_t width, size_t height)
+{
+    return width * height * 2;
+}

drone/hook/build_hook.sh

@@ -3,8 +3,14 @@
 echo "Building AR.Pwn Hook..."
 echo 
 
-"arm-none-linux-gnueabi-gcc.exe" -shared -fPIC -ldl -o libhook.so hook.c
+droneFlag=""
+
+if [ "$1" == "DRONEV2" ]; then
+    droneFlag="-D"$1
+fi
+
+"arm-none-linux-gnueabi-gcc.exe" -shared -fPIC -ldl -o libhook.so hook.c $droneFlag
 
 echo 
 echo "Build finished, Press Enter to exit."
-read DUMMY
+read DUMMY

drone/hook/hook.c

@@ -1,7 +1,7 @@
 /*
 
 AR.Pwn Hook Program
-Copyright 2012-2013 Jeremy Rand, Team SNARC / VECLabs
+Copyright 2012-2015 Jeremy Rand, Team SNARC / VECLabs
 Hooks the program.elf program from the AR.Drone firmware, and dumps interesting data relating to sensor devices.
 
 "arm-none-linux-gnueabi-gcc.exe" -shared -fPIC -ldl -o libhook.so hook.c
@@ -38,17 +38,23 @@ All calls for navdata
 #include <fcntl.h>
 #undef open
 
-#define VIDEO0_BUFFER "/tmp/video0_buffer"
-#define VIDEO0_READY "/tmp/video0_ready"
+#ifdef DRONEV2
+#include "CameraDroneV2.h"
+#else
+#include "CameraDroneV1.h"
+#endif
 
-#define VIDEO1_BUFFER "/tmp/video1_buffer"
-#define VIDEO1_READY "/tmp/video1_ready"
+#define CAM1_BUFFER "/tmp/video0_buffer"
+#define CAM1_READY "/tmp/video0_ready"
 
-#define VIDEO0_MARKED_BUFFER "/tmp/video0_marked_buffer"
-#define VIDEO0_MARKED_READY "/tmp/video0_marked_ready"
+#define CAM2_BUFFER "/tmp/video1_buffer"
+#define CAM2_READY "/tmp/video1_ready"
 
-#define VIDEO1_MARKED_BUFFER "/tmp/video1_marked_buffer"
-#define VIDEO1_MARKED_READY "/tmp/video1_marked_ready"
+#define CAM1_MARKED_BUFFER "/tmp/video0_marked_buffer"
+#define CAM1_MARKED_READY "/tmp/video0_marked_ready"
+
+#define CAM2_MARKED_BUFFER "/tmp/video1_marked_buffer"
+#define CAM2_MARKED_READY "/tmp/video1_marked_ready"
 
 int hook_handle_video0 = -1;
 //void* hook_buffer_video0 = NULL;
@@ -99,12 +105,12 @@ int open (__const char *__file, int __oflag, ...)
   open_handle = libc_open(__file, __oflag, mode);
   printf("AR.Pwn open() returned %d\n", open_handle);
 
-  if(strcmp(__file, "/dev/video0") == 0)
+  if(strcmp(__file, CAM1_DEVICE) == 0)
   {
     printf("AR.Pwn Detected open of video0; saving handle.\n");
 	hook_handle_video0 = open_handle;
   }
-  else if(strcmp(__file, "/dev/video1") == 0)
+  else if(strcmp(__file, CAM2_DEVICE) == 0)
   {
     printf("AR.Pwn Detected open of video1; saving handle.\n");
 	hook_handle_video1 = open_handle;
@@ -256,24 +262,24 @@ int ioctl(int d, int request, ...)
     printf("AR.Pwn Detected ioctl DQBUF of video0 index %d; dumping frame.\n", index);
 
 	// If Ready Flag is not present, or if buffer is not present
-	if( ! (access(VIDEO0_READY, F_OK) != -1) || ! (access(VIDEO0_BUFFER, F_OK) != -1) )
+	if( ! (access(CAM1_READY, F_OK) != -1) || ! (access(CAM1_BUFFER, F_OK) != -1) )
 	{
-		int buffer_file = open(VIDEO0_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
-		write(buffer_file, hook_buffers_video0[index], 640*480*3/2);
+		int buffer_file = open(CAM1_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+		write(buffer_file, hook_buffers_video0[index], getBufferSize(CAM1_WIDTH, CAM1_HEIGHT));
 		close(buffer_file);
 
-		int ready_file = open(VIDEO0_READY, O_WRONLY | O_CREAT, 0666);
+		int ready_file = open(CAM1_READY, O_WRONLY | O_CREAT, 0666);
 		close(ready_file);
 	}
 
 	// If a marked frame is available from the vision code...
-	if( (access(VIDEO0_MARKED_READY, F_OK) != -1))
+	if( (access(CAM1_MARKED_READY, F_OK) != -1))
 	{
-		int buffer_file = open(VIDEO0_MARKED_BUFFER, O_RDONLY);
-		read(buffer_file, hook_buffers_video0[index], 640*480*3/2);
+		int buffer_file = open(CAM1_MARKED_BUFFER, O_RDONLY);
+		read(buffer_file, hook_buffers_video0[index], getBufferSize(CAM1_WIDTH, CAM1_HEIGHT));
 		close(buffer_file);
 
-		remove(VIDEO0_MARKED_READY);
+		remove(CAM1_MARKED_READY);
 	}
 
 	// start is returned by mmap
@@ -289,26 +295,26 @@ int ioctl(int d, int request, ...)
     printf("AR.Pwn Detected ioctl DQBUF of video1 index %d; dumping frame.\n", index);
 
 	// If Ready Flag is not present, or if buffer is not present
-	if( ! (access(VIDEO1_READY, F_OK) != -1) || ! (access(VIDEO1_BUFFER, F_OK) != -1) )
+	if( ! (access(CAM2_READY, F_OK) != -1) || ! (access(CAM2_BUFFER, F_OK) != -1) )
 	{
-		int buffer_file = open(VIDEO1_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
-		write(buffer_file, hook_buffers_video1[index], 176*144*3/2);
+		int buffer_file = open(CAM2_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+		write(buffer_file, hook_buffers_video1[index], getBufferSize(CAM2_WIDTH, CAM2_HEIGHT));
 		close(buffer_file);
 
-		int ready_file = open(VIDEO1_READY, O_WRONLY | O_CREAT, 0666);
+		int ready_file = open(CAM2_READY, O_WRONLY | O_CREAT, 0666);
 		close(ready_file);
 	}
 
 	// If a marked frame is available from the vision code...
-	if( (access(VIDEO1_MARKED_READY, F_OK) != -1))
+	if( (access(CAM2_MARKED_READY, F_OK) != -1))
 	{
-		int buffer_file = open(VIDEO1_MARKED_BUFFER, O_RDONLY);
-		read(buffer_file, hook_buffers_video1[index], 176*144*3/2);
+		int buffer_file = open(CAM2_MARKED_BUFFER, O_RDONLY);
+		read(buffer_file, hook_buffers_video1[index], getBufferSize(CAM2_WIDTH, CAM2_HEIGHT));
 		close(buffer_file);
 
-		remove(VIDEO1_MARKED_READY);
+		remove(CAM2_MARKED_READY);
 	}
   }
 
   return ioctl_val;
-}
+}

drone/vision/telnet_interface.cpp

@@ -23,6 +23,12 @@ using namespace std;
 //#include "mb_vision/RGB24Image.h"
 #include "mb_vision/YUVImage.h"
 
+#ifdef DRONEV2
+#include "CameraDroneV2.h"
+#else
+#include "CameraDroneV1.h"
+#endif
+
 #define VIDEO0_BUFFER "/tmp/video0_buffer"
 #define VIDEO0_READY "/tmp/video0_ready"
 
@@ -155,8 +161,8 @@ int main()
     //printf("Start of main()\n");
 
 	// Chroma resolution is half of the raw resolution in each dimension
-	video0_visionImageWrapper = new YUVImage(640/2, 480/2);
-	video1_visionImageWrapper = new YUVImage(176/2, 144/2);
+	video0_visionImageWrapper = new YUVImage(CAM1_WIDTH/2, CAM1_HEIGHT/2);
+	video1_visionImageWrapper = new YUVImage(CAM2_WIDTH/2, CAM2_HEIGHT/2);
 
     //printf("Init vars\n");
 
@@ -236,7 +242,7 @@ int main()
 
 			//read_video0_yuv();
 			int buffer_file = open(VIDEO0_BUFFER, O_RDONLY);
-			read(buffer_file, video0_visionImageWrapper->getBuffer(), 640*480*3/2);
+			read(buffer_file, video0_visionImageWrapper->getBuffer(), getBufferSize(CAM1_WIDTH, CAM1_HEIGHT));
 			close(buffer_file);
 
 			//ConvertYUV2RGB(video0_yuv, video0_yuv+(640*480), video0_yuv+(640*480*5/4), video0_visionImageWrapper->getBuffer(), 640, 480);
@@ -303,7 +309,7 @@ int main()
 
 			//read_video0_yuv();
 			int buffer_file = open(VIDEO1_BUFFER, O_RDONLY);
-			read(buffer_file, video1_visionImageWrapper->getBuffer(), 176*144*3/2);
+			read(buffer_file, video1_visionImageWrapper->getBuffer(), getBufferSize(CAM2_WIDTH, CAM2_HEIGHT));
 			close(buffer_file);
 
 			//ConvertYUV2RGB(video0_yuv, video0_yuv+(640*480), video0_yuv+(640*480*5/4), video0_visionImageWrapper->getBuffer(), 640, 480);
@@ -378,7 +384,7 @@ int main()
             if( ! (access(VIDEO0_MARKED_READY, F_OK) != -1) || ! (access(VIDEO0_MARKED_BUFFER, F_OK) != -1) )
             {
                 int buffer_file = open(VIDEO0_MARKED_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
-                write(buffer_file, video0_visionImageWrapper->getBuffer(), 640*480*3/2);
+                write(buffer_file, video0_visionImageWrapper->getBuffer(), getBufferSize(CAM1_WIDTH, CAM1_HEIGHT));
                 close(buffer_file);
 
                 int ready_file = open(VIDEO0_MARKED_READY, O_WRONLY | O_CREAT, 0666);
@@ -392,7 +398,7 @@ int main()
             if( ! (access(VIDEO1_MARKED_READY, F_OK) != -1) || ! (access(VIDEO1_MARKED_BUFFER, F_OK) != -1) )
             {
                 int buffer_file = open(VIDEO1_MARKED_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666);
-                write(buffer_file, video1_visionImageWrapper->getBuffer(), 176*144*3/2);
+                write(buffer_file, video1_visionImageWrapper->getBuffer(), getBufferSize(CAM2_WIDTH, CAM2_HEIGHT));
                 close(buffer_file);
 
                 int ready_file = open(VIDEO1_MARKED_READY, O_WRONLY | O_CREAT, 0666);