diff --git a/C/Server/main.c b/C/Server/main.c
index 0a26214..8a6eaa1 100644
--- a/C/Server/main.c
+++ b/C/Server/main.c
@@ -68,11 +68,9 @@ int main(int argc, char* argv[])
   // draw...
   int x, y;
   uint8_t r, g, b, a;
-  int pixeloffset = 2;
+  int pixeloffset = 1;
   int pixellength = 0;
   
-
-  
   while( (packet_size = get_udp_packet(sock_fd, packet, sizeof(packet))) >= 0 ) {
     if((uint8_t)packet[0] == 1){
       pixellength = 8;
@@ -90,11 +88,11 @@ int main(int argc, char* argv[])
         i+=pixellength){
 	    x = (uint8_t)packet[i  ] + (((uint8_t)packet[i+1])<<8);
 	    y = (uint8_t)packet[i+2] + (((uint8_t)packet[i+3])<<8);
-	    r = packet[i+6];
+	    r = packet[i+4];
 	    g = packet[i+5];
-	    b = packet[i+4];
+	    b = packet[i+6];
 	    a = 0;
-      //printf("%d %d %d %d %d %d\n", x, y, packet[i  ], packet[i+1], packet[i+2], packet[i+3]);
+      //printf("%d %d %d %d %d %d\n", x, y, r, g, b, a);
       // Write pixel to screen
       write_pixel_to_screen(x, y, r, g, b, a);
     }
diff --git a/C/Server/pixelvloed b/C/Server/pixelvloed
new file mode 100755
index 0000000..6ec012e
Binary files /dev/null and b/C/Server/pixelvloed differ
diff --git a/C/Server/screenbuffer.c b/C/Server/screenbuffer.c
index 1ad7786..c69e14e 100644
--- a/C/Server/screenbuffer.c
+++ b/C/Server/screenbuffer.c
@@ -10,7 +10,6 @@
 #include <sys/ioctl.h>
 
 int fbfd = 0;
-struct fb_var_screeninfo orig_vinfo;
 struct fb_var_screeninfo vinfo;
 struct fb_fix_screeninfo finfo;
 long int screensize = 0;
@@ -32,19 +31,9 @@ int8_t init_frame_buffer(void)
     printf("Error reading variable information.\n");
     return 0;
   }
-  printf("Original %dx%d, %dbpp\n", vinfo.xres, vinfo.yres, 
+  printf("Screen info %dx%d, %dbpp\n", vinfo.xres, vinfo.yres, 
          vinfo.bits_per_pixel );
 
-  // Store for reset (copy vinfo to vinfo_orig)
-  memcpy(&orig_vinfo, &vinfo, sizeof(struct fb_var_screeninfo));
-
-  // Change variable info
-  vinfo.bits_per_pixel = 24;
-  if (ioctl(fbfd, FBIOPUT_VSCREENINFO, &vinfo)) {
-    printf("Error setting variable information.\n");
-    return 0;
-  }
-  
   // Get fixed screen information
   if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {
     printf("Error reading fixed information.\n");
@@ -72,19 +61,27 @@ void deinit_frame_buffer(void)
 {
   // cleanup
   munmap((void *)fbp, screensize);
-  if (ioctl(fbfd, FBIOPUT_VSCREENINFO, &orig_vinfo)) {
-    printf("Error re-setting variable information.\n");
-  }
   close(fbfd);
 }
 
-void write_pixel_to_screen(uint16_t x, uint16_t y, uint16_t r, uint16_t g, uint16_t b, uint8_t a){
-  unsigned int pix_offset;
-  unsigned bpp = vinfo.bits_per_pixel / 8;
+void write_pixel_to_screen(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a){
+  uint32_t pix_offset;
+  uint8_t bpp = vinfo.bits_per_pixel / 8;
+  union {
+    uint32_t pixel32;
+    uint8_t pixel8[4];
+  } pixel;
+  uint8_t i;
+  
   if(x < vinfo.xres && y < vinfo.yres){
+    pixel.pixel32 = 0;
+    pixel.pixel32 |= ((uint32_t)r >> (8 - vinfo.red.length)) << vinfo.red.offset;
+    pixel.pixel32 |= ((uint32_t)g >> (8 - vinfo.green.length)) << vinfo.green.offset;
+    pixel.pixel32 |= ((uint32_t)b >> (8 - vinfo.blue.length)) << vinfo.blue.offset;
+    
     pix_offset = bpp * x + y * finfo.line_length;
-    fbp[pix_offset++] = r;
-    fbp[pix_offset++] = g;
-    fbp[pix_offset] = b;
+    for (i=0; i<bpp; i++) {
+      fbp[pix_offset+i] = pixel.pixel8[i];
+    }
   }
 }
diff --git a/C/Server/screenbuffer.h b/C/Server/screenbuffer.h
index 315a5b4..cf449c0 100644
--- a/C/Server/screenbuffer.h
+++ b/C/Server/screenbuffer.h
@@ -6,6 +6,6 @@
 
 int8_t init_frame_buffer(void);
 void deinit_frame_buffer(void);
-void write_pixel_to_screen(uint16_t x, uint16_t y, uint16_t r, uint16_t g, uint16_t b, uint8_t a);
+void write_pixel_to_screen(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a);
 
 #endif