From 9112a3e9f7a4f61f60ed62bd8641163e93d90cd9 Mon Sep 17 00:00:00 2001 From: Jules Clero Date: Mon, 13 Jul 2015 23:29:14 +0000 Subject: [PATCH 1/3] Rename buffers for AR-Drone v2 support Buffers for vertical and horizontal camera are not the same for AR-Drone v1 and v2. This patch renames buffers' name to CAM1 and CAM2 to avoid hardware device association. Signed-off-by: Jules Clero --- drone/hook/hook.c | 44 ++++++++++++++++++++++---------------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/drone/hook/hook.c b/drone/hook/hook.c index 66807e6..ae532bb 100644 --- a/drone/hook/hook.c +++ b/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,17 @@ All calls for navdata #include #undef open -#define VIDEO0_BUFFER "/tmp/video0_buffer" -#define VIDEO0_READY "/tmp/video0_ready" +#define CAM1_BUFFER "/tmp/video0_buffer" +#define CAM1_READY "/tmp/video0_ready" -#define VIDEO1_BUFFER "/tmp/video1_buffer" -#define VIDEO1_READY "/tmp/video1_ready" +#define CAM2_BUFFER "/tmp/video1_buffer" +#define CAM2_READY "/tmp/video1_ready" -#define VIDEO0_MARKED_BUFFER "/tmp/video0_marked_buffer" -#define VIDEO0_MARKED_READY "/tmp/video0_marked_ready" +#define CAM1_MARKED_BUFFER "/tmp/video0_marked_buffer" +#define CAM1_MARKED_READY "/tmp/video0_marked_ready" -#define VIDEO1_MARKED_BUFFER "/tmp/video1_marked_buffer" -#define VIDEO1_MARKED_READY "/tmp/video1_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; @@ -256,24 +256,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); + int buffer_file = open(CAM1_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666); write(buffer_file, hook_buffers_video0[index], 640*480*3/2); 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); + int buffer_file = open(CAM1_MARKED_BUFFER, O_RDONLY); read(buffer_file, hook_buffers_video0[index], 640*480*3/2); close(buffer_file); - remove(VIDEO0_MARKED_READY); + remove(CAM1_MARKED_READY); } // start is returned by mmap @@ -289,26 +289,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); + int buffer_file = open(CAM2_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666); write(buffer_file, hook_buffers_video1[index], 176*144*3/2); 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); + int buffer_file = open(CAM2_MARKED_BUFFER, O_RDONLY); read(buffer_file, hook_buffers_video1[index], 176*144*3/2); close(buffer_file); - remove(VIDEO1_MARKED_READY); + remove(CAM2_MARKED_READY); } } return ioctl_val; -} \ No newline at end of file +} From 11da8be8347d0e49ffbd3a6a96531b08d75053ec Mon Sep 17 00:00:00 2001 From: Jules Clero Date: Wed, 15 Jul 2015 00:17:02 +0000 Subject: [PATCH 2/3] hook: Add support of ArDrone v2 The file device and the image size differs between ArDrone v1 and v2. This patch introduces different camera definition. ArDrone v1 definition is used by default. The ArDrone v2 one can be used by compiling with -DDRONEV2 flag. Signed-off-by: Jules Clero --- drone/hook/CameraDroneV1.h | 22 ++++++++++++++++++++++ drone/hook/CameraDroneV2.h | 22 ++++++++++++++++++++++ drone/hook/build_hook.sh | 10 ++++++++-- drone/hook/hook.c | 18 ++++++++++++------ 4 files changed, 64 insertions(+), 8 deletions(-) create mode 100644 drone/hook/CameraDroneV1.h create mode 100644 drone/hook/CameraDroneV2.h diff --git a/drone/hook/CameraDroneV1.h b/drone/hook/CameraDroneV1.h new file mode 100644 index 0000000..cd52aed --- /dev/null +++ b/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; +} + diff --git a/drone/hook/CameraDroneV2.h b/drone/hook/CameraDroneV2.h new file mode 100644 index 0000000..3487ef9 --- /dev/null +++ b/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; +} diff --git a/drone/hook/build_hook.sh b/drone/hook/build_hook.sh index 3d66beb..c10bc6b 100644 --- a/drone/hook/build_hook.sh +++ b/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 \ No newline at end of file +read DUMMY diff --git a/drone/hook/hook.c b/drone/hook/hook.c index ae532bb..37464f5 100644 --- a/drone/hook/hook.c +++ b/drone/hook/hook.c @@ -38,6 +38,12 @@ All calls for navdata #include #undef open +#ifdef DRONEV2 +#include "CameraDroneV2.h" +#else +#include "CameraDroneV1.h" +#endif + #define CAM1_BUFFER "/tmp/video0_buffer" #define CAM1_READY "/tmp/video0_ready" @@ -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; @@ -259,7 +265,7 @@ int ioctl(int d, int request, ...) if( ! (access(CAM1_READY, F_OK) != -1) || ! (access(CAM1_BUFFER, F_OK) != -1) ) { int buffer_file = open(CAM1_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666); - write(buffer_file, hook_buffers_video0[index], 640*480*3/2); + write(buffer_file, hook_buffers_video0[index], getBufferSize(CAM1_WIDTH, CAM1_HEIGHT)); close(buffer_file); int ready_file = open(CAM1_READY, O_WRONLY | O_CREAT, 0666); @@ -270,7 +276,7 @@ int ioctl(int d, int request, ...) if( (access(CAM1_MARKED_READY, F_OK) != -1)) { int buffer_file = open(CAM1_MARKED_BUFFER, O_RDONLY); - read(buffer_file, hook_buffers_video0[index], 640*480*3/2); + read(buffer_file, hook_buffers_video0[index], getBufferSize(CAM1_WIDTH, CAM1_HEIGHT)); close(buffer_file); remove(CAM1_MARKED_READY); @@ -292,7 +298,7 @@ int ioctl(int d, int request, ...) if( ! (access(CAM2_READY, F_OK) != -1) || ! (access(CAM2_BUFFER, F_OK) != -1) ) { int buffer_file = open(CAM2_BUFFER, O_WRONLY | O_CREAT | O_TRUNC, 0666); - write(buffer_file, hook_buffers_video1[index], 176*144*3/2); + write(buffer_file, hook_buffers_video1[index], getBufferSize(CAM2_WIDTH, CAM2_HEIGHT)); close(buffer_file); int ready_file = open(CAM2_READY, O_WRONLY | O_CREAT, 0666); @@ -303,7 +309,7 @@ int ioctl(int d, int request, ...) if( (access(CAM2_MARKED_READY, F_OK) != -1)) { int buffer_file = open(CAM2_MARKED_BUFFER, O_RDONLY); - read(buffer_file, hook_buffers_video1[index], 176*144*3/2); + read(buffer_file, hook_buffers_video1[index], getBufferSize(CAM2_WIDTH, CAM2_HEIGHT)); close(buffer_file); remove(CAM2_MARKED_READY); From eb51c6fc28f69a7e741029611fdaec3910f79f9f Mon Sep 17 00:00:00 2001 From: Jules Clero Date: Wed, 15 Jul 2015 00:39:59 +0000 Subject: [PATCH 3/3] vision: Use image size defined in hook Signed-off-by: Jules Clero --- drone/vision/telnet_interface.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/drone/vision/telnet_interface.cpp b/drone/vision/telnet_interface.cpp index 5fc470e..10ac420 100644 --- a/drone/vision/telnet_interface.cpp +++ b/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);