diff --git a/firmware/lib/it8951/it8951.h b/firmware/lib/it8951/it8951.h
index 01522a8..e01a8ac 100644
--- a/firmware/lib/it8951/it8951.h
+++ b/firmware/lib/it8951/it8951.h
@@ -454,8 +454,10 @@ void IT8951DisplayArea(uint16_t usX, uint16_t usY, uint16_t usW, uint16_t usH, u
   LCDWriteData(usDpyMode);
 }
 
-void IT8951_BMP_Example(uint32_t x, uint32_t y, uint32_t w, uint32_t h)
+void IT8951_BMP(uint32_t x, uint32_t y, uint32_t w, uint32_t h, uint8_t *image)
 {
+  gpFrameBuf = image;
+
   IT8951LdImgInfo stLdImgInfo;
   IT8951AreaImgInfo stAreaImgInfo;
   
@@ -475,6 +477,6 @@ void IT8951_BMP_Example(uint32_t x, uint32_t y, uint32_t w, uint32_t h)
   
   //Load Image from Host to IT8951 Image Buffer
   IT8951HostAreaPackedPixelWrite(&stLdImgInfo, &stAreaImgInfo);//Display function 2
-  //Display Area ?V (x,y,w,h) with mode 2 for fast gray clear mode - depends on current waveform 
-  //IT8951DisplayArea(0,0, gstI80DevInfo.usPanelW, gstI80DevInfo.usPanelH, 2);
+  //Display Area ?V (x,y,w,h) with mode 2 for fast gray clear mode - depends on current waveform
+  IT8951DisplayArea(0, 0, w, h, 2);
 }
diff --git a/firmware/src/displays/color7.h b/firmware/src/displays/color7.h
index 82d36ee..aed3377 100644
--- a/firmware/src/displays/color7.h
+++ b/firmware/src/displays/color7.h
@@ -53,18 +53,18 @@ void initDisplay()
 {
   if (epd.Init() != 0)
   {
-    Serial.print("e-Paper init failed");
+    Serial.println("e-Paper init failed");
     return;
   }
 }
 
 void drawImage(uint8_t *image)
 {
-  Serial.print("Wake up display\n");
+  Serial.println("Wake up display");
   epd.Reset();
-  Serial.print("Draw image\n");
+  Serial.println("Draw image");
   epd.EPD_7IN3F_Display(image);
-  Serial.print("Put display to sleep\n");
+  Serial.println("Put display to sleep");
   epd.Sleep();
 }
 #endif
\ No newline at end of file
diff --git a/firmware/src/displays/grey16.h b/firmware/src/displays/grey16.h
index bef593d..9e622b4 100644
--- a/firmware/src/displays/grey16.h
+++ b/firmware/src/displays/grey16.h
@@ -93,14 +93,15 @@ void initDisplay()
 
 void drawImage(uint8_t *image)
 {
-    gpFrameBuf = image;
-    Serial.println("Sending image");
-    IT8951_BMP_Example(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT);
-    Serial.println("Displaying image");
-    IT8951DisplayArea(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, 2);
-    Serial.println("Waiting for display ...");
+    Serial.println("Wake up display");
+    IT8951SystemRun();
+    Serial.println("Draw image");
+    IT8951_BMP(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, image);
+    Serial.println("Waiting for display to finish...");
     LCDWaitForReady();
-    Serial.println("done");
+    Serial.println("Done");
+    Serial.println("Putting display to sleep.");
+    IT8951Sleep();
 }
 
 #endif
\ No newline at end of file