diff --git a/companion/src/firmwares/opentx/opentxinterface.cpp b/companion/src/firmwares/opentx/opentxinterface.cpp
index a80796e31..f02325d9f 100644
--- a/companion/src/firmwares/opentx/opentxinterface.cpp
+++ b/companion/src/firmwares/opentx/opentxinterface.cpp
@@ -1237,7 +1237,7 @@ void registerOpenTxFirmwares()
 
   /* MEGA2560 board */
   openTx = new OpenTxFirmware("opentx-mega2560", QObject::tr("OpenTX for MEGA2560 board"), BOARD_MEGA2560);
-  Option LCD_options[] = { { "ST7565P LCD", QObject::tr("ST7565P LCD or compatible"), 0 }, { "ST7565R LCD", QObject::tr("ST7565R LCD or compatible"), 0 }, { NULL } };
+  Option LCD_options[] = { { "LCD_ST7565P", QObject::tr("ST7565P LCD or compatible"), 0 }, { "LCD_ST7565R", QObject::tr("ST7565R LCD or compatible"), 0 }, { NULL } };
   openTx->addOptions(LCD_options);
   openTx->addOption("heli", QObject::tr("Enable heli menu and cyclic mix support"));
   openTx->addOption("templates", QObject::tr("Enable TEMPLATES menu"));
diff --git a/radio/src/Makefile b/radio/src/Makefile
index e28de2bed..05513e418 100644
--- a/radio/src/Makefile
+++ b/radio/src/Makefile
@@ -672,11 +672,14 @@ ifeq ($(PCB), MEGA2560)
   BITMAPS += bitmaps/9X/splash.lbm bitmaps/9X/asterisk.lbm bitmaps/9X/about.lbm
   GUIGENERALSRC += gui/$(GUIDIRECTORY)/menu_general_diagkeys.cpp gui/$(GUIDIRECTORY)/menu_general_diaganas.cpp
 
-  ifeq ($(PCBREV), ST7565R)
-    EXTRABOARDSRC = targets/mega2560/lcd_driver_ST7565R.cpp targets/common_avr/telemetry_driver.cpp 
-  else
+  # LCD driver
+  # Values: ST7565R, ERC12864FSF, MULTIPLEX (none = ST7565P)
+  LCD = ST7565R
+  
+  ifeq ($(LCD), ST7565R)
+    CPPDEFS += -DLCD_ST7565R
+  endif   
     EXTRABOARDSRC = targets/stock/lcd_driver.cpp targets/common_avr/telemetry_driver.cpp
-  endif
 
   ifeq ($(SDCARD), YES)
     EXTRABOARDSRC += $(FATFSDIR)/ff.c $(FATFSDIR)/fattime.c $(FATFSDIR)/option/ccsbcs.c targets/gruvin9x/diskio.cpp
diff --git a/radio/src/targets/mega2560/lcd_driver_ST7565R.cpp b/radio/src/targets/mega2560/lcd_driver_ST7565R.cpp
deleted file mode 100644
index b0be5999c..000000000
--- a/radio/src/targets/mega2560/lcd_driver_ST7565R.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * Authors (alphabetical order)
- * - Andre Bernet <bernet.andre@gmail.com>
- * - Andreas Weitl
- * - Bertrand Songis <bsongis@gmail.com>
- * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
- * - Cameron Weeks <th9xer@gmail.com>
- * - Erez Raviv
- * - Gabriel Birkus
- * - Jean-Pierre Parisy
- * - Karl Szmutny
- * - Michael Blandford
- * - Michal Hlavinka
- * - Pat Mackenzie
- * - Philip Moss
- * - Rob Thomson
- * - Romolo Manfredini <romolo.manfredini@gmail.com>
- * - Thomas Husterer
- *
- * opentx is based on code named
- * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
- * er9x by Erez Raviv: http://code.google.com/p/er9x/,
- * and the original (and ongoing) project by
- * Thomas Husterer, th9x: http://code.google.com/p/th9x/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#include "opentx.h"
-
-#define delay_1us() _delay_us(1)
-void delay_1_5us(int ms)
-{
-  for(int i=0; i<ms; i++) delay_1us();
-}
-
-void lcdSendCtl(uint8_t val)
-{
-  PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_CS1);
-#ifdef LCD_MULTIPLEX
-  DDRA = 0xFF; // set LCD_DAT pins to output
-#endif
-  PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_A0);
-  PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_RnW);
-  PORTA_LCD_DAT = val;
-  PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_E);
-  PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_E);
-  PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_A0);
-#ifdef LCD_MULTIPLEX
-  DDRA = 0x00; // set LCD_DAT pins to input
-#endif
-  PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_CS1);
-}
-
-#if defined(PCBSTD) && defined(VOICE)
-volatile uint8_t LcdLock ;
-#define LCD_LOCK() LcdLock = 1
-#define LCD_UNLOCK() LcdLock = 0
-#else
-#define LCD_LOCK()
-#define LCD_UNLOCK()
-#endif
-
-inline void lcdInit()
-{
-  // /home/thus/txt/datasheets/lcd/KS0713.pdf
-  // ~/txt/flieger/ST7565RV17.pdf  from http://www.glyn.de/content.asp?wdid=132&sid=
-
-  LCD_LOCK();
-  PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_RES);  //LCD_RES
-  delay_1us();
-  delay_1us();//    f520  call  0xf4ce  delay_1us() ; 0x0xf4ce
-  PORTC_LCD_CTRL |= (1<<OUT_C_LCD_RES); //  f524  sbi 0x15, 2 IOADR-PORTC_LCD_CTRL; 21           1
-  delay_1_5us(1500);
-  lcdSendCtl(0xE2); //Initialize the internal functions
-  lcdSendCtl(0xAE); //DON = 0: display OFF
-  lcdSendCtl(0xA0); //ADC = 0: normal direction
-  lcdSendCtl(0xA6); //REV = 0: non-reverse display
-  lcdSendCtl(0xA4); //EON = 0: normal display. non-entire
-  lcdSendCtl(0xA2); //Select LCD bias=0
-  lcdSendCtl(0xC8); //SHL = 1: inverse direction
-  lcdSendCtl(0x2F); //Control power circuit operation VC=VR=VF=1
-  lcdSendCtl(0x25); //Select int resistance ratio R2 R1 R0 =5
-  lcdSendCtl(0x81); //Set reference voltage Mode
-  lcdSendCtl(0x22); //24 SV5 SV4 SV3 SV2 SV1 SV0 = 0x18
-  lcdSendCtl(0xAF); //DON = 1: display ON
-  lcdSendCtl(0x60); //sets the display start line to zero
-  g_eeGeneral.contrast = 0x22;
-  LCD_UNLOCK();
-}
-
-void lcdSetRefVolt(uint8_t val)
-{
-  LCD_LOCK();
-  lcdSendCtl(0x81);
-  lcdSendCtl(val);
-  LCD_UNLOCK();
-}
-
-void lcdRefresh()
-{
-  LCD_LOCK();
-  uint8_t *p=displayBuf;
-  for(uint8_t y=0; y < 8; y++) {
-    lcdSendCtl(0x01);
-    lcdSendCtl(0x10); //column addr 0
-    lcdSendCtl( y | 0xB0); //page addr y
-    PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_CS1);
-#ifdef LCD_MULTIPLEX
-    DDRA = 0xFF; // set LCD_DAT pins to output
-#endif
-    PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_A0);
-    PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_RnW);
-    for (coord_t x=LCD_W; x>0; --x) {
-      PORTA_LCD_DAT = *p++;
-      PORTC_LCD_CTRL |= (1<<OUT_C_LCD_E);
-      PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_E);
-    }
-    PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_A0);
-    PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_CS1);
-  }
-  LCD_UNLOCK();
-}
-
diff --git a/radio/src/targets/stock/lcd_driver.cpp b/radio/src/targets/stock/lcd_driver.cpp
index fcfc3c9ac..a5a125832 100644
--- a/radio/src/targets/stock/lcd_driver.cpp
+++ b/radio/src/targets/stock/lcd_driver.cpp
@@ -72,28 +72,47 @@ volatile uint8_t LcdLock ;
 
 const static pm_uchar lcdInitSequence[] PROGMEM =
 {
-   0xe2, //Initialize the internal functions
-   0xae, //DON = 0: display OFF
-   0xa1, //ADC = 1: reverse direction(SEG132->SEG1)
+#ifdef LCD_ST7565R
+   0xE2, //Initialize the internal functions
+   0xAE, //DON = 0: display OFF
+   0xA0, //ADC = 0: normal direction(SEG132->SEG1)
    0xA6, //REV = 0: non-reverse display
    0xA4, //EON = 0: normal display. non-entire
-#ifdef LCD_ERC12864FSF
-   0xA3, // Select LCD bias
+   0xA2, //Select LCD bias
+   0xC8, //SHL = 1: reverse direction (COM64->COM1)
+   0x2F, //Control power circuit operation VC=VR=VF=1
+   0x25, //Select int resistance ratio R2 R1 R0 =5
+   0x81, //Set reference voltage Mode
+   0x22, //24 SV5 SV4 SV3 SV2 SV1 SV0 = 0x18
+   0xAF, //DON = 1: display ON
+   0x60  //sets the display start line to zero
+#elif LCD_ERC12864FSF
+   0xE2, //Initialize the internal functions
+   0xAE, //DON = 0: display OFF
+   0xA1, //ADC = 1: reverse direction(SEG132->SEG1)
+   0xA6, //REV = 0: non-reverse display
+   0xA4, //EON = 0: normal display. non-entire
+   0xA3, //Select LCD bias
    0xC0, //SHL = 0: normal direction (COM1->COM64)
    0x2F, //Control power circuit operation VC=VR=VF=1
    0x27, //Select int resistance ratio R2 R1 R0
    0x81, //Set reference voltage Mode
-   0x2D, // 24 SV5 SV4 SV3 SV2 SV1 SV0
+   0x2D, //24 SV5 SV4 SV3 SV2 SV1 SV0
+   0xAF  //DON = 1: display ON
 #else
-   0xA2, // Select LCD bias=0
+   0xE2, //Initialize the internal functions
+   0xAE, //DON = 0: display OFF
+   0xA1, //ADC = 1: reverse direction(SEG132->SEG1)
+   0xA6, //REV = 0: non-reverse display
+   0xA4, //EON = 0: normal display. non-entire
+   0xA2, //Select LCD bias=0
    0xC0, //SHL = 0: normal direction (COM1->COM64)
    0x2F, //Control power circuit operation VC=VR=VF=1
    0x25, //Select int resistance ratio R2 R1 R0 =5
    0x81, //Set reference voltage Mode
-   0x22, // 24 SV5 SV4 SV3 SV2 SV1 SV0 = 0x18
-#endif
-
+   0x22, //24 SV5 SV4 SV3 SV2 SV1 SV0 = 0x18
    0xAF  //DON = 1: display ON
+#endif
 };
 
 inline void lcdInit()
@@ -106,7 +125,7 @@ inline void lcdInit()
   delay_2us();
   PORTC_LCD_CTRL |= (1<<OUT_C_LCD_RES); //  f524  sbi 0x15, 2 IOADR-PORTC_LCD_CTRL; 21           1
   delay_1_5us(1500);
-  for (uint8_t i=0; i<12; i++) {
+  for (uint8_t i=0; i<13; i++) {
     lcdSendCtl(pgm_read_byte(&lcdInitSequence[i])) ;
   }
 #ifdef LCD_ERC12864FSF
@@ -130,7 +149,13 @@ void lcdRefresh()
   LCD_LOCK();
   uint8_t *p=displayBuf;
   for(uint8_t y=0; y < 8; y++) {
+  
+#ifdef LCD_ST7565R
+    lcdSendCtl(0x01);
+#else
     lcdSendCtl(0x04);
+#endif
+
     lcdSendCtl(0x10); //column addr 0
     lcdSendCtl( y | 0xB0); //page addr y
     PORTC_LCD_CTRL &= ~(1<<OUT_C_LCD_CS1);
@@ -148,5 +173,6 @@ void lcdRefresh()
     PORTC_LCD_CTRL |=  (1<<OUT_C_LCD_CS1);
   }
   LCD_UNLOCK();
-}
+} 
 
+ 
\ No newline at end of file