Revised Pixel format (Motion-Project#756)

* YUV422P format
* RGB format

Author: Mr-DaveDev

video_bktr.c

@@ -590,11 +590,12 @@ static int bktr_capture(struct video_dev *viddev, unsigned char *map, int width,
         break;
     case METEOR_GEO_YUV_PACKED:
         /*FALLTHROUGH*/
-    case METEOR_GEO_YUV_PLANAR:
-        /*FALLTHROUGH*/
     case METEOR_GEO_YUV_422:
         vid_yuv422to420p(map, cap_map, width, height);
         break;
+    case METEOR_GEO_YUV_PLANAR:
+        vid_yuv422pto420p(map, cap_map, width, height);
+        break;
     case METEOR_GEO_YUV_9:
         /*FALLTHROUGH*/
     case METEOR_GEO_YUV_12:

video_common.c

@@ -288,6 +288,43 @@ void vid_yuv422to420p(unsigned char *map, unsigned char *cap_map, int width, int
     }
 }
 
+void vid_yuv422pto420p(unsigned char *map, unsigned char *cap_map, int width, int height)
+{
+    unsigned char *src, *dest, *dest2;
+    unsigned char *src_u, *src_u2, *src_v, *src_v2;
+
+    int i, j;
+    /*Planar version of 422 */
+    /* Create the Y plane. */
+    src = cap_map;
+    dest = map;
+    for (i = width * height; i > 0; i--) {
+        *dest++ = *src++;
+    }
+
+    /* Create U and V planes. */
+    dest = map + width * height;
+    dest2 = dest + (width * height) / 4;
+    for (i = 0; i< (height / 2)-2; i++) {
+        src_u = cap_map + (width * height) + ((i*2) * (width/2));
+        src_u2 = src_u  + (width/2);
+        src_v = src_u + (width/2 * height);
+        src_v2 = src_v  + (width/2);
+
+        for (j = 0; j < (width / 2); j++) {
+            *dest = ((int) *src_u + (int) *src_u2) / 2;
+            src_u ++;
+            src_u2++;
+            dest++;
+
+            *dest2 = ((int) *src_v + (int) *src_v2) / 2;
+            src_v ++;
+            src_v2++;
+            dest2++;
+        }
+    }
+}
+
 void vid_uyvyto420p(unsigned char *map, unsigned char *cap_map, int width, int height)
 {
     uint8_t *pY = map;
@@ -329,9 +366,10 @@ void vid_rgb24toyuv420p(unsigned char *map, unsigned char *cap_map, int width, i
     unsigned char *r, *g, *b;
     int i, loop;
 
-    b = cap_map;
-    g = b + 1;
-    r = g + 1;
+    r = cap_map;
+    g = r + 1;
+    b = g + 1;
+
     y = map;
     u = y + width * height;
     v = u + (width * height) / 4;

video_common.h

@@ -59,6 +59,7 @@ void vid_mutex_init(void);
 int vid_parms_parse(struct context *cnt);
 
 void vid_yuv422to420p(unsigned char *map, unsigned char *cap_map, int width, int height);
+void vid_yuv422pto420p(unsigned char *map, unsigned char *cap_map, int width, int height);
 void vid_uyvyto420p(unsigned char *map, unsigned char *cap_map, int width, int height);
 void vid_rgb24toyuv420p(unsigned char *map, unsigned char *cap_map, int width, int height);
 void vid_bayer2rgb24(unsigned char *dst, unsigned char *src, long int width, long int height);

video_v4l2.c

@@ -1068,10 +1068,11 @@ static int v4l2_capture(struct context *cnt, struct video_dev *curdev, unsigned
             return 0;
 
         case V4L2_PIX_FMT_YUYV:
-            /*FALLTHROUGH*/
-        case V4L2_PIX_FMT_YUV422P:
             vid_yuv422to420p(map, the_buffer->ptr, width, height);
             return 0;
+        case V4L2_PIX_FMT_YUV422P:
+            vid_yuv422pto420p(map, the_buffer->ptr, width, height);
+            return 0;
 
         case V4L2_PIX_FMT_YUV420:
             memcpy(map, the_buffer->ptr, the_buffer->content_length);