Separate oled hardware init routine from open

add LCD ClearInit which also re-inits the hardware

workaround for display corruption on startup

src/SSD1306.cpp

@@ -32,16 +32,18 @@ SSD1306::SSD1306(int BSCMaster, u8 address, int flip, int type)
 	, address(address)
 	, type(type)
 	, flip(flip)
+	, contrast(127)
 {
 	RPI_I2CInit(BSCMaster, 1);
 
-	SetupScreen();
+	InitHardware();
 }
 
-void SSD1306::SetupScreen()
+void SSD1306::InitHardware()
 {
 	// SSD1306 data sheet configuration flow
 	SendCommand(SSD1306_CMD_DISPLAY_OFF);	// 0xAE
+
 	SendCommand(SSD1306_CMD_MULTIPLEX_RATIO);  // 0xA8
 	SendCommand(0x3F);	// SSD1306_LCDHEIGHT - 1
 
@@ -61,8 +63,7 @@ void SSD1306::SetupScreen()
 	SendCommand(SSD1306_CMD_SET_COM_PINS);	// 0xDA Layout and direction
 	SendCommand(0x12);
 
-	SendCommand(SSD1306_CMD_SET_CONTRAST_CONTROL);
+	SetContrast(GetContrast());
-	SendCommand(0x7F);
 
 	SendCommand(SSD1306_CMD_ENTIRE_DISPLAY_ON);
 
@@ -176,6 +177,7 @@ void SSD1306::DisplayOff()
 
 void SSD1306::SetContrast(u8 value)
 {
+	contrast = value;
 	SendCommand(SSD1306_CMD_SET_CONTRAST_CONTROL);
 	SendCommand(value);
 	SetVCOMDeselect( value >> 5);

src/SSD1306.h

@@ -81,10 +81,11 @@ public:
 	void PlotCharacter(int x, int y, char ascii, bool inverse);
 	void Plottext(int x, int y, char* str, bool inverse);
 
-	void SetupScreen();
+	void InitHardware();
 	void DisplayOn();
 	void DisplayOff();
 	void SetContrast(u8 value);
+	u8 GetContrast() { return contrast; }
 	void SetVCOMDeselect(u8 value);
 
 	void ClearScreen();
@@ -108,6 +109,7 @@ protected:
 	u8 address;
 	int type;
 	int flip;
+	int contrast;
 };
 #endif
 

src/Screen.cpp

@@ -337,3 +337,4 @@ void Screen::PlotImage(u32* image, int x, int y, int w, int h)
 		}
 	}
 }
+

src/ScreenLCD.cpp

@@ -61,6 +61,14 @@ void ScreenLCD::Clear(RGBA colour)
 	ssd1306->ClearScreen();
 }
 
+void ScreenLCD::ClearInit(RGBA colour)
+{
+	ssd1306->InitHardware();
+	ssd1306->ClearScreen();
+	ssd1306->SetContrast(ssd1306->GetContrast());
+	ssd1306->DisplayOn();
+}
+
 void ScreenLCD::SetContrast(u8 value)
 {
 	ssd1306->SetContrast(value);

src/ScreenLCD.h

@@ -36,6 +36,7 @@ public:
 
 	void ClearArea(u32 x1, u32 y1, u32 x2, u32 y2, RGBA colour);
 	void Clear(RGBA colour);
+	void ClearInit(RGBA colour);
 
 	void SetContrast(u8 value);
 

src/main.cpp

@@ -364,6 +364,7 @@ void InitialiseLCD()
 	{
 		screenLCD = new ScreenLCD();
 		screenLCD->Open(128, 64, 1, i2cBusMaster, i2cLcdAddress, i2cLcdFlip, i2cLcdModel);
+		screenLCD->SetContrast(i2cLcdOnContrast);
 
 		bool logo_done = false;
 		if (strcasecmp(options.GetLcdLogoName(), "1541ii") == 0)
@@ -402,8 +403,6 @@ void InitialiseLCD()
 			screenLCD->PrintText(0, x, y, tempBuffer, 0x0);
 		}
 		screenLCD->RefreshScreen();
-
-		screenLCD->SetContrast(i2cLcdOnContrast);
 	}
 }
 
@@ -1283,6 +1282,8 @@ extern "C"
 
 		IEC_Bus::Initialise();
 
+		screenLCD->ClearInit(0);
+
 #ifdef HAS_MULTICORE
 		start_core(3, _spin_core);
 		start_core(2, _spin_core);