camera: giga: fix display example for rgb sensors

The STM32 DMA is unable to modify the endianness of the 16bit data being tranferred, so we need to do it manually. Anyway, it's not a big deal since we already needed to copy every file for scaling+transpose feature..

Author: facchinm

Diff for: libraries/Camera/examples/GigaCameraDisplay/GigaCameraDisplay.ino

@@ -22,13 +22,11 @@ Camera cam(himax);
 OV7675 ov767x;
 Camera cam(ov767x);
 #define IMAGE_MODE CAMERA_RGB565
-#error "Unsupported camera (at the moment :) )"
 #elif defined(ARDUCAM_CAMERA_GC2145)
 #include "GC2145/gc2145.h"
 GC2145 galaxyCore;
 Camera cam(galaxyCore);
 #define IMAGE_MODE CAMERA_RGB565
-#error "Unsupported camera (at the moment :) )"
 #endif
 
 // The buffer used to capture the frame
@@ -63,9 +61,11 @@ void setup() {
   }
 
   Display.begin();
-  dsi_configueCLUT((uint32_t*)palette);
 
-  outfb.setBuffer((uint8_t*)SDRAM.malloc(1024*1024));
+  if (IMAGE_MODE == CAMERA_GRAYSCALE) {
+    dsi_configueCLUT((uint32_t*)palette);
+  }
+  outfb.setBuffer((uint8_t*)SDRAM.malloc(1024 * 1024));
 
   // clear the display (gives a nice black background)
   dsi_lcdClear(0);
@@ -74,6 +74,8 @@ void setup() {
   dsi_drawCurrentFrameBuffer();
 }
 
+#define HTONS(x)    (((x >> 8) & 0x00FF) | ((x << 8) & 0xFF00))
+
 void loop() {
 
   // Grab frame and write to another framebuffer
@@ -83,13 +85,20 @@ void loop() {
     // this only works if the camera feed is 320x240 and the area where we want to display is 640x480
     for (int i = 0; i < 320; i++) {
       for (int j = 0; j < 240; j++) {
-        ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480] = ((uint8_t*)fb.getBuffer())[i + j * 320];
-        ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480 + 1] = ((uint8_t*)fb.getBuffer())[i + j * 320];
-        ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480] = ((uint8_t*)fb.getBuffer())[i + j * 320];
-        ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480 + 1] = ((uint8_t*)fb.getBuffer())[i + j * 320];
+        if (IMAGE_MODE == CAMERA_GRAYSCALE) {
+          ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480] = ((uint8_t*)fb.getBuffer())[i + j * 320];
+          ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480 + 1] = ((uint8_t*)fb.getBuffer())[i + j * 320];
+          ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480] = ((uint8_t*)fb.getBuffer())[i + j * 320];
+          ((uint8_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480 + 1] = ((uint8_t*)fb.getBuffer())[i + j * 320];
+        } else {
+          ((uint16_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480] = HTONS(((uint16_t*)fb.getBuffer())[i + j * 320]);
+          ((uint16_t*)outfb.getBuffer())[j * 2 + (i * 2) * 480 + 1] = HTONS(((uint16_t*)fb.getBuffer())[i + j * 320]);
+          ((uint16_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480] = HTONS(((uint16_t*)fb.getBuffer())[i + j * 320]);
+          ((uint16_t*)outfb.getBuffer())[j * 2 + (i * 2 + 1) * 480 + 1] = HTONS(((uint16_t*)fb.getBuffer())[i + j * 320]);
+        }
       }
     }
-    dsi_lcdDrawImage((void*)outfb.getBuffer(), (void*)dsi_getCurrentFrameBuffer(), 480, 640, DMA2D_INPUT_L8);
+    dsi_lcdDrawImage((void*)outfb.getBuffer(), (void*)dsi_getCurrentFrameBuffer(), 480, 640, IMAGE_MODE == CAMERA_GRAYSCALE ? DMA2D_INPUT_L8 : DMA2D_INPUT_RGB565);
     dsi_drawCurrentFrameBuffer();
   } else {
     blinkLED(20);