diff --git a/conf/modules/cv_opencvdemo.xml b/conf/modules/cv_opencvdemo.xml
index 0003baac0e..803975f989 100644
--- a/conf/modules/cv_opencvdemo.xml
+++ b/conf/modules/cv_opencvdemo.xml
@@ -15,7 +15,7 @@
   <makefile>
     <file name="cv_opencvdemo.c"/>
     <file name="opencv_example.cpp"/>
-
+    <file name="opencv_image_functions.cpp"/>
     <flag name="CXXFLAGS" value="I$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install/include"/>
 
   	<flag name="LDFLAGS" value="L$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install/lib" />
diff --git a/sw/airborne/modules/computer_vision/cv_opencvdemo.c b/sw/airborne/modules/computer_vision/cv_opencvdemo.c
index ca40e5086e..5cd5d891e1 100644
--- a/sw/airborne/modules/computer_vision/cv_opencvdemo.c
+++ b/sw/airborne/modules/computer_vision/cv_opencvdemo.c
@@ -20,7 +20,7 @@
 /**
  * @file "modules/computer_vision/cv_opencvdemo.c"
  * @author C. De Wagter
- * opencv
+ * A simple module showing what you can do with opencv on the bebop.
  */
 
 #include "modules/computer_vision/cv.h"
diff --git a/sw/airborne/modules/computer_vision/opencv_example.cpp b/sw/airborne/modules/computer_vision/opencv_example.cpp
index 107892e94e..08b5a3d154 100644
--- a/sw/airborne/modules/computer_vision/opencv_example.cpp
+++ b/sw/airborne/modules/computer_vision/opencv_example.cpp
@@ -20,7 +20,7 @@
 /**
  * @file "modules/computer_vision/opencv_example.cpp"
  * @author C. De Wagter
- * opencv
+ * A simple module showing what you can do with opencv on the bebop.
  */
 
 
@@ -32,7 +32,7 @@ using namespace std;
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 using namespace cv;
-
+#include "opencv_image_functions.h"
 
 int opencv_example(char *img, int width, int height)
 {
@@ -52,11 +52,6 @@ int opencv_example(char *img, int width, int height)
   Canny(image, image, edgeThresh, edgeThresh * 3);
 
   // Convert back to YUV422, and put it in place of the original image
-  for (int row = 0; row < height; row++) {
-    for (int col = 0; col < width; col++) {
-      img[(row * width + col) * 2 + 1] = image.at<uint8_t>(row, col);
-      img[(row * width + col) * 2] = 127;
-    }
-  }
+  grayscale_opencv_to_yuv422(image, img, width, height);
   return 0;
 }
diff --git a/sw/airborne/modules/computer_vision/opencv_example.h b/sw/airborne/modules/computer_vision/opencv_example.h
index 5450b965f6..46c648ec39 100644
--- a/sw/airborne/modules/computer_vision/opencv_example.h
+++ b/sw/airborne/modules/computer_vision/opencv_example.h
@@ -20,7 +20,7 @@
 /**
  * @file "modules/computer_vision/cv_opencvdemo.h"
  * @author C. De Wagter
- * opencv
+ * A simple module showing what you can do with opencv on the bebop.
  */
 
 #ifndef OPENCV_EXAMPLE_H
diff --git a/sw/airborne/modules/computer_vision/opencv_image_functions.cpp b/sw/airborne/modules/computer_vision/opencv_image_functions.cpp
new file mode 100644
index 0000000000..a80154fa1c
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/opencv_image_functions.cpp
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2016 Roland Meertens
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/computer_vision/opencv_image_functions.cpp
+ *
+ * A small library with functions to convert between the Paparazzi used YUV422 arrays
+ * and the opencv image functions.
+ */
+
+#include "opencv_image_functions.h"
+
+
+using namespace std;
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+using namespace cv;
+
+void color_opencv_to_yuv422(Mat image, char *img, int width, int height)
+{
+
+//Turn the opencv RGB colored image back in a YUV colored image for the drone
+  for (int row = 0; row < height; row++) {
+    for (int col = 0; col < width; col++) {
+      cv::Vec3b pixelHere = image.at<cv::Vec3b>(row, col);
+      img[(row * width + col) * 2 + 1] = 0.299 * pixelHere[0] + 0.587 * pixelHere[1] + 0.114 * pixelHere[2];
+      if (col % 2 == 0) { // U
+        img[(row * width + col) * 2] = 0.492 * (pixelHere[2] - img[(row * width + col) * 2 + 1] + 127);
+      } else { // V
+        img[(row * width + col) * 2] = 0.877 * (pixelHere[0] - img[(row * width + col) * 2 + 1] + 127);
+      }
+    }
+  }
+}
+
+
+void grayscale_opencv_to_yuv422(Mat image, char *img, int width, int height)
+{
+  for (int row = 0; row < height; row++) {
+    for (int col = 0; col < width; col++) {
+
+      img[(row * width + col) * 2 + 1] =   image.at<uint8_t>(row, col);
+      img[(row * width + col) * 2 ] = 127;
+    }
+  }
+}
diff --git a/sw/airborne/modules/computer_vision/opencv_image_functions.h b/sw/airborne/modules/computer_vision/opencv_image_functions.h
new file mode 100644
index 0000000000..36fd286ab9
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/opencv_image_functions.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2016 Roland Meertens
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/computer_vision/opencv_image_functions.h
+ *
+ * A small library with functions to convert between the Paparazzi used YUV422 arrays
+ * and the opencv image functions.
+ */
+
+#ifndef OPENCV_IMAGE_FUNCTIONS_H
+#define OPENCV_IMAGE_FUNCTIONS_H
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+void color_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
+void grayscale_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
+#endif
diff --git a/sw/ext/opencv_bebop b/sw/ext/opencv_bebop
index 9eb660dde0..03cce41a32 160000
--- a/sw/ext/opencv_bebop
+++ b/sw/ext/opencv_bebop
@@ -1 +1 @@
-Subproject commit 9eb660dde0f7ec919d476645e136318e74b1a276
+Subproject commit 03cce41a32f43d50e294e7a2e486626b568f4588