add UTF-8 recode support, Cyrilic rus/ukr tables

Dependents:   Current_load1

Fork of TextLCD by Wim Huiskamp

TextLCD Library UTF-8 recode support, Cyrilic rus/ukr tables

Revision:
41:51a77c5a45b6
Parent:
40:d3496c3ea301
Child:
43:63e5deba31ff
--- a/TextLCD.cpp	Wed May 20 18:46:51 2015 +0000
+++ b/TextLCD.cpp	Sun Oct 11 13:37:14 2015 +0000
@@ -1,25 +1,25 @@
 /* mbed TextLCD Library, for LCDs based on HD44780 controllers
  * Copyright (c) 2007-2010, sford, http://mbed.org
- *               2013, v01: WH, Added LCD types, fixed LCD address issues, added Cursor and UDCs 
- *               2013, v02: WH, Added I2C and SPI bus interfaces  
- *               2013, v03: WH, Added support for LCD40x4 which uses 2 controllers 
+ *               2013, v01: WH, Added LCD types, fixed LCD address issues, added Cursor and UDCs
+ *               2013, v02: WH, Added I2C and SPI bus interfaces
+ *               2013, v03: WH, Added support for LCD40x4 which uses 2 controllers
  *               2013, v04: WH, Added support for Display On/Off, improved 4bit bootprocess
- *               2013, v05: WH, Added support for 8x2B, added some UDCs   
- *               2013, v06: WH, Added support for devices that use internal DC/DC converters 
- *               2013, v07: WH, Added support for backlight and include portdefinitions for LCD2004 Module from DFROBOT 
- *               2014, v08: WH, Refactored in Base and Derived Classes to deal with mbed lib change regarding 'NC' defined pins 
- *               2014, v09: WH/EO, Added Class for Native SPI controllers such as ST7032 
- *               2014, v10: WH, Added Class for Native I2C controllers such as ST7032i, Added support for MCP23008 I2C portexpander, Added support for Adafruit module  
- *               2014, v11: WH, Added support for native I2C controllers such as PCF21XX, Improved the _initCtrl() method to deal with differences between all supported controllers  
- *               2014, v12: WH, Added support for native I2C controller PCF2119 and native I2C/SPI controllers SSD1803, ST7036, added setContrast method (by JH1PJL) for supported devices (eg ST7032i) 
- *               2014, v13: WH, Added support for controllers US2066/SSD1311 (OLED), added setUDCBlink() method for supported devices (eg SSD1803), fixed issue in setPower() 
- *               2014, v14: WH, Added support for PT6314 (VFD), added setOrient() method for supported devices (eg SSD1803, US2066), added Double Height lines for supported devices, 
+ *               2013, v05: WH, Added support for 8x2B, added some UDCs
+ *               2013, v06: WH, Added support for devices that use internal DC/DC converters
+ *               2013, v07: WH, Added support for backlight and include portdefinitions for LCD2004 Module from DFROBOT
+ *               2014, v08: WH, Refactored in Base and Derived Classes to deal with mbed lib change regarding 'NC' defined pins
+ *               2014, v09: WH/EO, Added Class for Native SPI controllers such as ST7032
+ *               2014, v10: WH, Added Class for Native I2C controllers such as ST7032i, Added support for MCP23008 I2C portexpander, Added support for Adafruit module
+ *               2014, v11: WH, Added support for native I2C controllers such as PCF21XX, Improved the _initCtrl() method to deal with differences between all supported controllers
+ *               2014, v12: WH, Added support for native I2C controller PCF2119 and native I2C/SPI controllers SSD1803, ST7036, added setContrast method (by JH1PJL) for supported devices (eg ST7032i)
+ *               2014, v13: WH, Added support for controllers US2066/SSD1311 (OLED), added setUDCBlink() method for supported devices (eg SSD1803), fixed issue in setPower()
+ *               2014, v14: WH, Added support for PT6314 (VFD), added setOrient() method for supported devices (eg SSD1803, US2066), added Double Height lines for supported devices,
  *                              added 16 UDCs for supported devices (eg PCF2103), moved UDC defines to TextLCD_UDC file, added TextLCD_Config.h for feature and footprint settings.
- *               2014, v15: WH, Added AC780 support, added I2C expander modules, fixed setBacklight() for inverted logic modules. Fixed bug in LCD_SPI_N define 
- *               2014, v16: WH, Added ST7070 and KS0073 support, added setIcon(), clrIcon() and setInvert() method for supported devices 
- *               2015, v17: WH, Clean up low-level _writeCommand() and _writeData(), Added support for alternative fonttables (eg PCF21XX), Added ST7066_ACM controller for ACM1602 module 
+ *               2014, v15: WH, Added AC780 support, added I2C expander modules, fixed setBacklight() for inverted logic modules. Fixed bug in LCD_SPI_N define
+ *               2014, v16: WH, Added ST7070 and KS0073 support, added setIcon(), clrIcon() and setInvert() method for supported devices
+ *               2015, v17: WH, Clean up low-level _writeCommand() and _writeData(), Added support for alternative fonttables (eg PCF21XX), Added ST7066_ACM controller for ACM1602 module
  *               2015, v18: WH, Performance improvement I2C portexpander
- *               2015, v19: WH, Fixed Adafruit I2C/SPI portexpander pinmappings, fixed SYDZ Backlight 
+ *               2015, v19: WH, Fixed Adafruit I2C/SPI portexpander pinmappings, fixed SYDZ Backlight
  *               2015, v20: WH, Fixed occasional Init fail caused by insufficient wait time after ReturnHome command (0x02), Added defines to reduce memory footprint (eg LCD_ICON),
  *                              Fixed and Added more fonttable support for PCF2119R_3V3, Added HD66712 controller.
  *
@@ -44,1449 +44,1462 @@
 #include "mbed.h"
 #include "TextLCD.h"
 #include "TextLCD_UDC.inc"
-   
+
+Serial pc1(USBTX,USBRX);
+
+
 /** Create a TextLCD_Base interface
   *
   * @param type  Sets the panel size/addressing mode (default = LCD16x2)
-  * @param ctrl  LCD controller (default = HD44780)           
+  * @param ctrl  LCD controller (default = HD44780)
   */
-TextLCD_Base::TextLCD_Base(LCDType type, LCDCtrl ctrl) : _type(type), _ctrl(ctrl) {
-    
-  // Extract LCDType data  
-
-  // Columns encoded in b7..b0
-  _nr_cols = (_type & 0xFF);          
-
-  // Rows encoded in b15..b8  
-  _nr_rows = ((_type >> 8) & 0xFF);  
-
-  // Addressing mode encoded in b19..b16  
-  _addr_mode = _type & LCD_T_ADR_MSK;
-  
-  // Font table, encoded in LCDCtrl  
-  _font = _ctrl & LCD_C_FNT_MSK;
+TextLCD_Base::TextLCD_Base(LCDType type, LCDCtrl ctrl) : _type(type), _ctrl(ctrl)
+{
+    // Extract LCDType data
+
+    // Columns encoded in b7..b0
+    _nr_cols = (_type & 0xFF);
+
+    // Rows encoded in b15..b8
+    _nr_rows = ((_type >> 8) & 0xFF);
+
+    // Addressing mode encoded in b19..b16
+    _addr_mode = _type & LCD_T_ADR_MSK;
+
+    // Font table, encoded in LCDCtrl
+    _font = _ctrl & LCD_C_FNT_MSK;
 }
 
 /**  Init the LCD Controller(s)
-  *  Clear display 
+  *  Clear display
   *  @param _LCDDatalength dl sets the datalength of data/commands
-  *  @return none 
+  *  @return none
   */
-void TextLCD_Base::_init(_LCDDatalength dl) {
-
-  wait_ms(100);                  // Wait 100ms to ensure powered up
-  
-  // Select and configure second LCD controller when needed
-  if(_type==LCD40x4) {
-    _ctrl_idx=_LCDCtrl_1;        // Select 2nd controller   
-    _initCtrl(dl);               // Init 2nd controller   
-  }
-    
-  // Select and configure primary LCD controller
-  _ctrl_idx=_LCDCtrl_0;          // Select primary controller  
-  _initCtrl(dl);                 // Init primary controller
-
-  // Clear whole display and Reset Cursor location
-  // Note: This will make sure that some 3-line displays that skip topline of a 4-line configuration 
-  //       are cleared and init cursor correctly.
-  cls();   
-} 
+void TextLCD_Base::_init(_LCDDatalength dl)
+{
+
+    wait_ms(100);                  // Wait 100ms to ensure powered up
+
+    // Select and configure second LCD controller when needed
+    if(_type==LCD40x4) {
+        _ctrl_idx=_LCDCtrl_1;        // Select 2nd controller
+        _initCtrl(dl);               // Init 2nd controller
+    }
+
+    // Select and configure primary LCD controller
+    _ctrl_idx=_LCDCtrl_0;          // Select primary controller
+    _initCtrl(dl);                 // Init primary controller
+
+    // Clear whole display and Reset Cursor location
+    // Note: This will make sure that some 3-line displays that skip topline of a 4-line configuration
+    //       are cleared and init cursor correctly.
+    cls();
+}
 
 /**  Init the LCD controller
   *   Set number of lines, fonttype, no cursor etc
   *   The controller is accessed in 4-bit parallel mode either directly via mbed pins or through I2C or SPI expander.
-  *   Some controllers also support native I2C or SPI interfaces. 
+  *   Some controllers also support native I2C or SPI interfaces.
   *
   *  @param _LCDDatalength dl sets the 4 or 8 bit datalength of data/commands. Required for some native serial modes.
-  *  @return none  
+  *  @return none
   *
   *  Note: some configurations are commented out because they have not yet been tested due to lack of hardware
   */
-void TextLCD_Base::_initCtrl(_LCDDatalength dl) {
-  int _bias_lines=0; // Set Bias and lines (Instr Set 1), temporary variable.
-  int _lines=0;      // Set lines (Ext Instr Set), temporary variable.
+void TextLCD_Base::_initCtrl(_LCDDatalength dl)
+{
+    int _bias_lines=0; // Set Bias and lines (Instr Set 1), temporary variable.
+    int _lines=0;      // Set lines (Ext Instr Set), temporary variable.
 
     this->_setRS(false); // command mode
 
     if (dl == _LCD_DL_4) {
-      // The Controller could be in 8 bit mode (power-on reset) or in 4 bit mode (warm reboot) at this point.
-      // Follow this procedure to make sure the Controller enters the correct state. The hardware interface
-      // between the uP and the LCD can only write the 4 most significant bits (Most Significant Nibble, MSN).
-      // In 4 bit mode the LCD expects the MSN first, followed by the LSN.
-      //
-      //    Current state:               8 bit mode                |      4 bit mode, MSN is next        | 4 bit mode, LSN is next          
-                           //-------------------------------------------------------------------------------------------------                          
-      _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, |   set 8 bit mode (MSN),             |    set dummy LSN, 
-                           //  remains in 8 bit mode               |    remains in 4 bit mode            |  remains in 4 bit mode
-      wait_ms(15);         //                           
-     
-      _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, |      set dummy LSN,                 |    set 8bit mode (MSN), 
-                           //  remains in 8 bit mode               |   change to 8 bit mode              |  remains in 4 bit mode
-      wait_ms(15);         // 
-    
-      _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, | set 8 bit mode (MSN) and dummy LSN, |    set dummy LSN, 
-                           //  remains in 8 bit mode               |   remains in 8 bit mode             |  change to 8 bit mode
-      wait_ms(15);         // 
-
-      // Controller is now in 8 bit mode
-
-      _writeNibble(0x2);   // Change to 4-bit mode (MSN), the LSN is undefined dummy
-      wait_us(40);         // most instructions take 40us
-
-      // Controller is now in 4-bit mode
-      // Note: 4/8 bit mode is ignored for most native SPI and I2C devices. They dont use the parallel bus.
-      //       However, _writeNibble() method is void anyway for native SPI and I2C devices.
+        // The Controller could be in 8 bit mode (power-on reset) or in 4 bit mode (warm reboot) at this point.
+        // Follow this procedure to make sure the Controller enters the correct state. The hardware interface
+        // between the uP and the LCD can only write the 4 most significant bits (Most Significant Nibble, MSN).
+        // In 4 bit mode the LCD expects the MSN first, followed by the LSN.
+        //
+        //    Current state:               8 bit mode                |      4 bit mode, MSN is next        | 4 bit mode, LSN is next
+        //-------------------------------------------------------------------------------------------------
+        _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, |   set 8 bit mode (MSN),             |    set dummy LSN,
+        //  remains in 8 bit mode               |    remains in 4 bit mode            |  remains in 4 bit mode
+        wait_ms(15);         //
+
+        _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, |      set dummy LSN,                 |    set 8bit mode (MSN),
+        //  remains in 8 bit mode               |   change to 8 bit mode              |  remains in 4 bit mode
+        wait_ms(15);         //
+
+        _writeNibble(0x3);   //  set 8 bit mode (MSN) and dummy LSN, | set 8 bit mode (MSN) and dummy LSN, |    set dummy LSN,
+        //  remains in 8 bit mode               |   remains in 8 bit mode             |  change to 8 bit mode
+        wait_ms(15);         //
+
+        // Controller is now in 8 bit mode
+
+        _writeNibble(0x2);   // Change to 4-bit mode (MSN), the LSN is undefined dummy
+        wait_us(40);         // most instructions take 40us
+
+        // Controller is now in 4-bit mode
+        // Note: 4/8 bit mode is ignored for most native SPI and I2C devices. They dont use the parallel bus.
+        //       However, _writeNibble() method is void anyway for native SPI and I2C devices.
+    } else {
+        // Reset in 8 bit mode, final Function set will follow
+        _writeCommand(0x30); // Function set 0 0 1 DL=1 N F x x
+        wait_ms(1);          // most instructions take 40us
     }
-    else {
-      // Reset in 8 bit mode, final Function set will follow 
-      _writeCommand(0x30); // Function set 0 0 1 DL=1 N F x x       
-      wait_ms(1);          // most instructions take 40us      
-    }      
-   
+
     // Device specific initialisations: DC/DC converter to generate VLCD or VLED, number of lines etc
     switch (_ctrl) {
 
-      case KS0073:
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD12x1:                                
-            case LCD16x1:                                            
-            case LCD20x1:
-            case LCD24x1:
+        case KS0073:
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD12x1:
+                case LCD16x1:
+                case LCD20x1:
+                case LCD24x1:
 //            case LCD32x1:        // EXT pin is High, extension driver needed
-//            case LCD40x1:        // EXT pin is High, extension driver needed                        
-              _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
-                                    //   DH=1  (Disp shift enable, special mode for KS0073)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0073)
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)                                
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)                                
-              break;                                
-
-//            case LCD12x3D:         // Special mode for KS0073, KS0078 and PCF21XX            
-//            case LCD12x3D1:        // Special mode for KS0073, KS0078 and PCF21XX            
-            case LCD12x4D:         // Special mode for KS0073, KS0078 and PCF21XX            
-//            case LCD16x3D:         // Special mode for KS0073, KS0078             
-//            case LCD16x4D:         // Special mode for KS0073, KS0078            
-            case LCD20x4D:         // Special mode for KS0073, KS0078            
-              _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (dont care for 4 line mode)              
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
-                                    //   DH=1  (Disp shift enable, special mode for KS0073)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0073)
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode), N=1 (2-line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)                                
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)                                    
-
-              _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)                                
-              break;                                
-
-
-            case LCD16x3G:            // Special mode for ST7036                        
+//            case LCD40x1:        // EXT pin is High, extension driver needed
+                    _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
+                    //   DH=1  (Disp shift enable, special mode for KS0073)
+                    //   REV=0 (Reverse normal, special mode for KS0073)
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)
+                    break;
+
+//            case LCD12x3D:         // Special mode for KS0073, KS0078 and PCF21XX
+//            case LCD12x3D1:        // Special mode for KS0073, KS0078 and PCF21XX
+                case LCD12x4D:         // Special mode for KS0073, KS0078 and PCF21XX
+//            case LCD16x3D:         // Special mode for KS0073, KS0078
+//            case LCD16x4D:         // Special mode for KS0073, KS0078
+                case LCD20x4D:         // Special mode for KS0073, KS0078
+                    _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (dont care for 4 line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
+                    //   DH=1  (Disp shift enable, special mode for KS0073)
+                    //   REV=0 (Reverse normal, special mode for KS0073)
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode), N=1 (2-line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+                    _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)
+                    break;
+
+
+                case LCD16x3G:            // Special mode for ST7036
 //            case LCD24x3D:         // Special mode for KS0078
 //            case LCD24x3D1:        // Special mode for KS0078
-            case LCD24x4D:         // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)            
-              _function  = 0x0A;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (2-line mode), N=0 (1-line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
-                                    //   DH=1  (Disp shift enable, special mode for KS0073)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0073)
-                                    
-              _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (2 line mode), N=0 (1-line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)                   
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)                                                                                     
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)                                
-              break;
-          } // switch type
-
-          // init special features
-          _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                           //   DL=0 (4 bits bus), DL=1 (8 bits mode)            
-                                           //    N=0 (1 line mode), N=1 (2 line mode)
-                                           //   RE=1 (Ena Extended Regs, special mode for KS0073)
-                                           //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for KS0073)                                
-                                           //   LP=0  (LP=1 Low power mode, LP=0 Normal)                                                                                                                                
-
-          _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                           //   FW=0  (5-dot font, special mode for KS0073)
-                                           //   BW=0  (Cur BW invert disable, special mode for KS0073)
-                                           //   NW=0  (1,2 Line), NW=1 (4 line, special mode for KS0073)                                
-
-          _writeCommand(0x10);             // Scroll/Shift set 0001 DS/HS4 DS/HS3 DS/HS2 DS/HS1 (Ext Regs)
-                                           //   Dotscroll/Display shift enable (Special mode for KS0073)
-
-          _writeCommand(0x80);             // Scroll Quantity set 1 0 SQ5 SQ4 SQ3 SQ2 SQ1 SQ0 (Ext Regs)
-                                           //   Scroll quantity (Special mode for KS0073)
-
-          _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                           //   DL=0  (4 bits bus), DL=1 (8 bits mode)             
-                                           //    N=0  (1 line mode), N=1 (2 line mode)
-                                           //   RE=0  (Dis. Extended Regs, special mode for KS0073)
-                                           //   DH=1  (Disp shift enable/disable, special mode for KS0073)                                
-                                           //   REV=0 (Reverse/Normal, special mode for KS0073)
-          break; // case KS0073 Controller
-
-
-      case KS0078:
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2B is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:                                            
+                case LCD24x4D:         // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function  = 0x0A;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (2-line mode), N=0 (1-line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0073)
+                    //   DH=1  (Disp shift enable, special mode for KS0073)
+                    //   REV=0 (Reverse normal, special mode for KS0073)
+
+                    _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (2 line mode), N=0 (1-line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0073)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0073)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0073)
+                    break;
+            } // switch type
+
+            // init special features
+            _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE LP (Ext Regs)
+            //   DL=0 (4 bits bus), DL=1 (8 bits mode)
+            //    N=0 (1 line mode), N=1 (2 line mode)
+            //   RE=1 (Ena Extended Regs, special mode for KS0073)
+            //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for KS0073)
+            //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+            _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
+            //   FW=0  (5-dot font, special mode for KS0073)
+            //   BW=0  (Cur BW invert disable, special mode for KS0073)
+            //   NW=0  (1,2 Line), NW=1 (4 line, special mode for KS0073)
+
+            _writeCommand(0x10);             // Scroll/Shift set 0001 DS/HS4 DS/HS3 DS/HS2 DS/HS1 (Ext Regs)
+            //   Dotscroll/Display shift enable (Special mode for KS0073)
+
+            _writeCommand(0x80);             // Scroll Quantity set 1 0 SQ5 SQ4 SQ3 SQ2 SQ1 SQ0 (Ext Regs)
+            //   Scroll quantity (Special mode for KS0073)
+
+            _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
+            //   DL=0  (4 bits bus), DL=1 (8 bits mode)
+            //    N=0  (1 line mode), N=1 (2 line mode)
+            //   RE=0  (Dis. Extended Regs, special mode for KS0073)
+            //   DH=1  (Disp shift enable/disable, special mode for KS0073)
+            //   REV=0 (Reverse/Normal, special mode for KS0073)
+            break; // case KS0073 Controller
+
+
+        case KS0078:
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2B is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
 //            case LCD20x1:
-            case LCD24x1:
-              _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1 line mode), N=1 (2 line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
-                                    //   DH=1  (Disp shift enable, special mode for KS0078)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0078)
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1 line mode), N=1 (2 line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)                                
-                                    //      0 
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)                                
-              break;                                
-
-//            case LCD12x3D:         // Special mode for KS0073, KS0078 and PCF21XX            
-//            case LCD12x3D1:        // Special mode for KS0073, KS0078 and PCF21XX            
-//            case LCD12x4D:         // Special mode for KS0073, KS0078 and PCF21XX            
-//            case LCD16x3D:         // Special mode for KS0073, KS0078             
-//            case LCD16x4D:         // Special mode for KS0073, KS0078            
-//            case LCD20x4D:         // Special mode for KS0073, KS0078            
+                case LCD24x1:
+                    _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1 line mode), N=1 (2 line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
+                    //   DH=1  (Disp shift enable, special mode for KS0078)
+                    //   REV=0 (Reverse normal, special mode for KS0078)
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1 line mode), N=1 (2 line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)
+                    //      0
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)
+                    break;
+
+//            case LCD12x3D:         // Special mode for KS0073, KS0078 and PCF21XX
+//            case LCD12x3D1:        // Special mode for KS0073, KS0078 and PCF21XX
+//            case LCD12x4D:         // Special mode for KS0073, KS0078 and PCF21XX
+//            case LCD16x3D:         // Special mode for KS0073, KS0078
+//            case LCD16x4D:         // Special mode for KS0073, KS0078
+//            case LCD20x4D:         // Special mode for KS0073, KS0078
 //            case LCD24x3D:         // Special mode for KS0078
 //            case LCD24x3D1:        // Special mode for KS0078
-            case LCD24x4D:         // Special mode for KS0078
-              _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (dont care for 4 line mode)              
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
-                                    //   DH=1  (Disp shift enable, special mode for KS0078)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0078)
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1 line mode), N=1 (2 line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)                                
-                                    //      0 
-
-              _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)                                
-              break;                                
-
-            case LCD16x3G:            // Special mode for ST7036                        
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-              
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)            
-              _function  = 0x0A;    // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (1 line mode), N=1 (2 line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
-                                    //   DH=1  (Disp shift enable, special mode for KS0078)                                
-                                    //   REV=0 (Reverse normal, special mode for KS0078)
-                                    
-              _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (1 line mode), N=1 (2 line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)                                
-                                    //      0 
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)                                
-              break;
-          } // switch type
-
-          // init special features
-          _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE 0 (Ext Regs)
-                                           //   DL=0 (4 bits bus), DL=1 (8 bits mode)            
-                                           //    N=0 (1 line mode), N=1 (2 line mode)
-                                           //   RE=1 (Ena Extended Regs, special mode for KS0078)
-                                           //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for KS0078)                                
-                                           //      0 
-
-          _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                           //   FW=0  (5-dot font, special mode for KS0078)
-                                           //   BW=0  (Cur BW invert disable, special mode for KS0078)
-                                           //   NW=0  (1,2 Line), NW=1 (4 line, special mode for KS0078)                                
-
-          _writeCommand(0x10);             // Scroll/Shift set 0001 DS/HS4 DS/HS3 DS/HS2 DS/HS1 (Ext Regs)
-                                           //   Dotscroll/Display shift enable (Special mode for KS0078)
-
-          _writeCommand(0x80);             // Scroll Quantity set 1 0 SQ5 SQ4 SQ3 SQ2 SQ1 SQ0 (Ext Regs)
-                                           //   Scroll quantity (Special mode for KS0078)
-
-          _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                           //   DL=0  (4 bits bus), DL=1 (8 bits mode)             
-                                           //    N=0  (1 line mode), N=1 (2 line mode)
-                                           //   RE=0  (Dis. Extended Regs, special mode for KS0078)
-                                           //   DH=1  (Disp shift enable/disable, special mode for KS0078)                                
-                                           //   REV=0 (Reverse/Normal, special mode for KS0078)
-          break; // case KS0078 Controller
-              
-      case ST7032_3V3:
-          // ST7032 controller: Initialise Voltage booster for VLCD. VDD=3V3
-      case ST7032_5V:
-          // ST7032 controller: Disable Voltage booster for VLCD. VDD=5V
-
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2B is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:                                            
-//            case LCD20x1:                    
-            case LCD24x1:
-              _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 (4 bit), N=0 (1-line display mode), F=0 (5*7dot), 0, IS
-                                      // Note: 4 bit mode is ignored for native SPI and I2C devices
-                                      // Saved to allow switch between Instruction sets at later time
-              break;  
-
-            case LCD12x3D:            // Special mode for KS0078 and PCF21XX
-            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
-            case LCD12x4D:            // Special mode for KS0078 and PCF21XX
-            case LCD16x3G:            // Special mode for ST7036                        
-            case LCD24x4D:            // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays        
-              _function = 0x08;       // FUNCTION SET 0 0 1 DL=0 (4 bit), N=1 (2-line display mode), F=0 (5*7dot), 0, IS              
-                                      // Note: 4 bit mode is ignored for native SPI and I2C devices
-                                      // Saved to allow switch between Instruction sets at later time
-              break;                                                                        
-          } // switch type    
-                                     
-          // init special features 
-          _writeCommand(0x20 | _function | 0x01);           // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1              
-
-          _writeCommand(0x1C);                              // Internal OSC frequency adjustment Framefreq=183HZ, Bias will be 1/4 (Instr Set=1)
-
-          _contrast = LCD_ST7032_CONTRAST;              
-          _writeCommand(0x70 | (_contrast & 0x0F));         // Set Contrast Low bits, 0 1 1 1 C3 C2 C1 C0 (IS=1)
-
-
-          if (_ctrl == ST7032_3V3) {
+                case LCD24x4D:         // Special mode for KS0078
+                    _function  = 0x02;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (dont care for 4 line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
+                    //   DH=1  (Disp shift enable, special mode for KS0078)
+                    //   REV=0 (Reverse normal, special mode for KS0078)
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1 line mode), N=1 (2 line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)
+                    //      0
+
+                    _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)
+                    break;
+
+                case LCD16x3G:            // Special mode for ST7036
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function  = 0x0A;    // Function set 001 DL N RE(0) DH REV (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (1 line mode), N=1 (2 line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for KS0078)
+                    //   DH=1  (Disp shift enable, special mode for KS0078)
+                    //   REV=0 (Reverse normal, special mode for KS0078)
+
+                    _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE 0 (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (1 line mode), N=1 (2 line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for KS0078)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for KS0078)
+                    //      0
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for KS0078)
+                    break;
+            } // switch type
+
+            // init special features
+            _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE 0 (Ext Regs)
+            //   DL=0 (4 bits bus), DL=1 (8 bits mode)
+            //    N=0 (1 line mode), N=1 (2 line mode)
+            //   RE=1 (Ena Extended Regs, special mode for KS0078)
+            //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for KS0078)
+            //      0
+
+            _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
+            //   FW=0  (5-dot font, special mode for KS0078)
+            //   BW=0  (Cur BW invert disable, special mode for KS0078)
+            //   NW=0  (1,2 Line), NW=1 (4 line, special mode for KS0078)
+
+            _writeCommand(0x10);             // Scroll/Shift set 0001 DS/HS4 DS/HS3 DS/HS2 DS/HS1 (Ext Regs)
+            //   Dotscroll/Display shift enable (Special mode for KS0078)
+
+            _writeCommand(0x80);             // Scroll Quantity set 1 0 SQ5 SQ4 SQ3 SQ2 SQ1 SQ0 (Ext Regs)
+            //   Scroll quantity (Special mode for KS0078)
+
+            _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
+            //   DL=0  (4 bits bus), DL=1 (8 bits mode)
+            //    N=0  (1 line mode), N=1 (2 line mode)
+            //   RE=0  (Dis. Extended Regs, special mode for KS0078)
+            //   DH=1  (Disp shift enable/disable, special mode for KS0078)
+            //   REV=0 (Reverse/Normal, special mode for KS0078)
+            break; // case KS0078 Controller
+
+        case ST7032_3V3:
+            // ST7032 controller: Initialise Voltage booster for VLCD. VDD=3V3
+        case ST7032_5V:
+            // ST7032 controller: Disable Voltage booster for VLCD. VDD=5V
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2B is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+//            case LCD20x1:
+                case LCD24x1:
+                    _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 (4 bit), N=0 (1-line display mode), F=0 (5*7dot), 0, IS
+                    // Note: 4 bit mode is ignored for native SPI and I2C devices
+                    // Saved to allow switch between Instruction sets at later time
+                    break;
+
+                case LCD12x3D:            // Special mode for KS0078 and PCF21XX
+                case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
+                case LCD12x4D:            // Special mode for KS0078 and PCF21XX
+                case LCD16x3G:            // Special mode for ST7036
+                case LCD24x4D:            // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays
+                    _function = 0x08;       // FUNCTION SET 0 0 1 DL=0 (4 bit), N=1 (2-line display mode), F=0 (5*7dot), 0, IS
+                    // Note: 4 bit mode is ignored for native SPI and I2C devices
+                    // Saved to allow switch between Instruction sets at later time
+                    break;
+            } // switch type
+
+            // init special features
+            _writeCommand(0x20 | _function | 0x01);           // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1
+
+            _writeCommand(0x1C);                              // Internal OSC frequency adjustment Framefreq=183HZ, Bias will be 1/4 (Instr Set=1)
+
+            _contrast = LCD_ST7032_CONTRAST;
+            _writeCommand(0x70 | (_contrast & 0x0F));         // Set Contrast Low bits, 0 1 1 1 C3 C2 C1 C0 (IS=1)
+
+
+            if (_ctrl == ST7032_3V3) {
 //            _icon_power = 0x04;                             // Icon display off, Booster circuit is turned on  (IS=1)
-            _icon_power = 0x0C;                             // Icon display on, Booster circuit is turned on  (IS=1)            
-                                                            // Saved to allow contrast change at later time
-          }
-          else { 
+                _icon_power = 0x0C;                             // Icon display on, Booster circuit is turned on  (IS=1)
+                // Saved to allow contrast change at later time
+            } else {
 //            _icon_power = 0x00;                             // Icon display off, Booster circuit is turned off  (IS=1)
-            _icon_power = 0x08;                             // Icon display on, Booster circuit is turned off  (IS=1)            
-                                                            // Saved to allow contrast change at later time
-          }
-          _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));  // Set Icon, Booster and Contrast High bits, 0 1 0 1 Ion Bon C5 C4 (IS=1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-          
-          _writeCommand(0x68 | (LCD_ST7032_RAB & 0x07));      // Voltage follower, 0 1 1 0 FOn=1, Ampl ratio Rab2=1, Rab1=0, Rab0=0  (IS=1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-          
-          _writeCommand(0x20 | _function);                  // Select Instruction Set = 0
-
-          break; // case ST7032_3V3 Controller
-                 // case ST7032_5V Controller
-
-      case ST7036_3V3:
-          // ST7036 controller: Initialise Voltage booster for VLCD. VDD=3V3
-          // Note: supports 1,2 (LCD_T_A) or 3 lines (LCD_T_G)
-      case ST7036_5V:
-          // ST7036 controller: Disable Voltage booster for VLCD. VDD=5V
-          // Note: supports 1,2 (LCD_T_A) or 3 lines (LCD_T_G)
-                    
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2D is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:   
-            case LCD24x1:                                                                         
-              _function = 0x00;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=0 (1 Line), DH=0 (5x7font), IS2, IS1 (Select Instruction Set)
-                                    // Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    // Saved to allow switch between Instruction sets at later time
-              
-              _bias_lines = 0x04;   // Bias: 1/5, 1 or 2-Lines LCD 
-              break;  
+                _icon_power = 0x08;                             // Icon display on, Booster circuit is turned off  (IS=1)
+                // Saved to allow contrast change at later time
+            }
+            _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));  // Set Icon, Booster and Contrast High bits, 0 1 0 1 Ion Bon C5 C4 (IS=1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x68 | (LCD_ST7032_RAB & 0x07));      // Voltage follower, 0 1 1 0 FOn=1, Ampl ratio Rab2=1, Rab1=0, Rab0=0  (IS=1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x20 | _function);                  // Select Instruction Set = 0
+
+            break; // case ST7032_3V3 Controller
+            // case ST7032_5V Controller
+
+        case ST7036_3V3:
+            // ST7036 controller: Initialise Voltage booster for VLCD. VDD=3V3
+            // Note: supports 1,2 (LCD_T_A) or 3 lines (LCD_T_G)
+        case ST7036_5V:
+            // ST7036 controller: Disable Voltage booster for VLCD. VDD=5V
+            // Note: supports 1,2 (LCD_T_A) or 3 lines (LCD_T_G)
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2D is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+                case LCD24x1:
+                    _function = 0x00;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=0 (1 Line), DH=0 (5x7font), IS2, IS1 (Select Instruction Set)
+                    // Note: 4 bit mode is ignored for native SPI and I2C devices
+                    // Saved to allow switch between Instruction sets at later time
+
+                    _bias_lines = 0x04;   // Bias: 1/5, 1 or 2-Lines LCD
+                    break;
 
 //            case LCD12x3G:          // Special mode for ST7036
-            case LCD16x3G:          // Special mode for ST7036
-              _function = 0x08;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=1 (2 Line), DH=0 (5x7font), IS2,IS1 (Select Instruction Set)              
-                                    // Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    // Saved to allow switch between Instruction sets at later time
-              
-              _bias_lines = 0x05;   // Bias: 1/5, 3-Lines LCD           
-              break;  
+                case LCD16x3G:          // Special mode for ST7036
+                    _function = 0x08;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=1 (2 Line), DH=0 (5x7font), IS2,IS1 (Select Instruction Set)
+                    // Note: 4 bit mode is ignored for native SPI and I2C devices
+                    // Saved to allow switch between Instruction sets at later time
+
+                    _bias_lines = 0x05;   // Bias: 1/5, 3-Lines LCD
+                    break;
 
 //            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
 //            case LCD16x3D1:           // Special mode for SSD1803
-            case LCD12x4D:            // Special mode for PCF2116
-            case LCD24x4D:            // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)       
-              _function = 0x08;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=1 (2 Line), DH=0 (5x7font), IS2,IS1 (Select Instruction Set)
-                                    // Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    // Saved to allow switch between Instruction sets at later time
-              
-              _bias_lines = 0x04;   // Bias: 1/5, 1 or 2-Lines LCD 
-              break;                
-          } // switch type
-
-
-          // init special features 
-          _writeCommand(0x20 | _function | 0x01);   // Set function, IS2,IS1 = 01 (Select Instr Set = 1)
-          _writeCommand(0x10 | _bias_lines);        // Set Bias and 1,2 or 3 lines (Instr Set 1)
-
-          _contrast = LCD_ST7036_CONTRAST;
-          _writeCommand(0x70 | (_contrast & 0x0F)); // Set Contrast, 0 1 1 1 C3 C2 C1 C0 (Instr Set 1)
-                           
-          if (_ctrl == ST7036_3V3) {
-            _icon_power = 0x0C;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=1 Bon=1 C5 C4 (Instr Set 1)            
+                case LCD12x4D:            // Special mode for PCF2116
+                case LCD24x4D:            // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function = 0x08;     // Set function, 0 0 1 DL=0 (4-bit Databus), N=1 (2 Line), DH=0 (5x7font), IS2,IS1 (Select Instruction Set)
+                    // Note: 4 bit mode is ignored for native SPI and I2C devices
+                    // Saved to allow switch between Instruction sets at later time
+
+                    _bias_lines = 0x04;   // Bias: 1/5, 1 or 2-Lines LCD
+                    break;
+            } // switch type
+
+
+            // init special features
+            _writeCommand(0x20 | _function | 0x01);   // Set function, IS2,IS1 = 01 (Select Instr Set = 1)
+            _writeCommand(0x10 | _bias_lines);        // Set Bias and 1,2 or 3 lines (Instr Set 1)
+
+            _contrast = LCD_ST7036_CONTRAST;
+            _writeCommand(0x70 | (_contrast & 0x0F)); // Set Contrast, 0 1 1 1 C3 C2 C1 C0 (Instr Set 1)
+
+            if (_ctrl == ST7036_3V3) {
+                _icon_power = 0x0C;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=1 Bon=1 C5 C4 (Instr Set 1)
 //            _icon_power = 0x04;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=0 Bon=1 C5 C4 (Instr Set 1)
-                                                      // Saved to allow contrast change at later time
-          } 
-          else {
-            _icon_power = 0x08;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=1 Bon=0 C5 C4 (Instr Set 1)             
-//            _icon_power = 0x00;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=0 Bon=0 C5 C4 (Instr Set 1)                         
-          }
-          
-          _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));   // Set Contrast C5, C4 (Instr Set 1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-
-          _writeCommand(0x68 | (LCD_ST7036_RAB & 0x07));  // Voltagefollower On = 1, Ampl ratio Rab2, Rab1, Rab0 = 1 0 1 (Instr Set 1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-
-          _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instruction Set = 0)
-         
-          break; // case ST7036_3V3 Controller
-                 // case ST7036_5V Controller
-
-      case ST7070:                   
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2D is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:   
-            case LCD24x1:                                                                         
-              _function = dl | 0x00;      // Set function, 0 0 1 DL=0 (4-bit Databus), N=0 (1 Line), EXT=0, x, x 
-                                          // Note: 4 bit mode is NOT ignored for native SPI !
-                                          // Saved to allow switch between Instruction sets at later time
-              break;  
+                // Saved to allow contrast change at later time
+            } else {
+                _icon_power = 0x08;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=1 Bon=0 C5 C4 (Instr Set 1)
+//            _icon_power = 0x00;                       // Set Icon, Booster, Contrast High bits, 0 1 0 1 Ion=0 Bon=0 C5 C4 (Instr Set 1)
+            }
+
+            _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));   // Set Contrast C5, C4 (Instr Set 1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x68 | (LCD_ST7036_RAB & 0x07));  // Voltagefollower On = 1, Ampl ratio Rab2, Rab1, Rab0 = 1 0 1 (Instr Set 1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instruction Set = 0)
+
+            break; // case ST7036_3V3 Controller
+            // case ST7036_5V Controller
+
+        case ST7070:
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2D is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+                case LCD24x1:
+                    _function = dl | 0x00;      // Set function, 0 0 1 DL=0 (4-bit Databus), N=0 (1 Line), EXT=0, x, x
+                    // Note: 4 bit mode is NOT ignored for native SPI !
+                    // Saved to allow switch between Instruction sets at later time
+                    break;
 
 //            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
 //            case LCD16x3D1:           // Special mode for SSD1803
-            case LCD12x4D:            // Special mode for PCF2116
-            case LCD24x4D:            // Special mode for KS0078
+                case LCD12x4D:            // Special mode for PCF2116
+                case LCD24x4D:            // Special mode for KS0078
 //            case LCD12x3G:          // Special mode for ST7036
-            case LCD16x3G:          // Special mode for ST7036           
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)       
-              _function = dl | 0x08;   // Set function, 0 0 1 DL, N=1 (2 Line), EXT=0, x, x                                                                 
-                                       // Note: 4 bit mode is NOT ignored for native SPI !
-                                       // Saved to allow switch between Instruction sets at later time
-              break;                
-          } // switch type
+                case LCD16x3G:          // Special mode for ST7036
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function = dl | 0x08;   // Set function, 0 0 1 DL, N=1 (2 Line), EXT=0, x, x
+                    // Note: 4 bit mode is NOT ignored for native SPI !
+                    // Saved to allow switch between Instruction sets at later time
+                    break;
+            } // switch type
 
 //          _writeCommand(0x00);                      // NOP, make sure to sync SPI
-          
-          // init special features                                                    
-          _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1)
-
-          _writeCommand(0x04 | 0x00);               // Set Bias resistors  0 0 0 0 0 1 Rb1,Rb0= 0 0 (Extern Res) (Instr Set 1)
-
-          _writeCommand(0x40 | 0x00);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
-                                                    // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
-                                                    // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41                                                    
-          
-          _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
-         
-          break; // case ST7070 Controller
-         
-      case SSD1803_3V3:
-          // SSD1803 controller: Initialise Voltage booster for VLCD. VDD=3V3
-          // Note: supports 1,2, 3 or 4 lines
+
+            // init special features
+            _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1)
+
+            _writeCommand(0x04 | 0x00);               // Set Bias resistors  0 0 0 0 0 1 Rb1,Rb0= 0 0 (Extern Res) (Instr Set 1)
+
+            _writeCommand(0x40 | 0x00);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
+            // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
+            // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41
+
+            _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
+
+            break; // case ST7070 Controller
+
+        case SSD1803_3V3:
+            // SSD1803 controller: Initialise Voltage booster for VLCD. VDD=3V3
+            // Note: supports 1,2, 3 or 4 lines
 //      case SSD1803_5V:
-          // SSD1803 controller: No Voltage booster for VLCD. VDD=5V
-                    
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2D is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:   
-            case LCD24x1:                                                                         
-              _function = 0x00;     //  Set function 0 0 1 DL N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=0 1 Line / 3 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x02;   // Set function, 0 0 1 DL N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=0 1 Line / 3 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803
-                                    //   REV=0 Reverse off, special feature of SSD1803            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=0 1-Line LCD (N=0)
-              break;  
-
-            case LCD12x3D:          // Special mode for KS0078 and PCF21XX                                  
-//            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX            
-            case LCD16x3D:          // Special mode for KS0078
+            // SSD1803 controller: No Voltage booster for VLCD. VDD=5V
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2D is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+                case LCD24x1:
+                    _function = 0x00;     //  Set function 0 0 1 DL N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=0 1 Line / 3 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x02;   // Set function, 0 0 1 DL N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=0 1 Line / 3 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803
+                    //   REV=0 Reverse off, special feature of SSD1803
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=0 1-Line LCD (N=0)
+                    break;
+
+                case LCD12x3D:          // Special mode for KS0078 and PCF21XX
+//            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
+                case LCD16x3D:          // Special mode for KS0078
 //            case LCD16x3D1:           // Special mode for SSD1803
 //            case LCD20x3D:            // Special mode for SSD1803
-              _function = 0x00;     //  Set function 0 0 1 DL N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=0 1 Line / 3 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x02;   // Set function, 0 0 1 DL N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=0 1 Line / 3 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803
-                                    //   REV=0 Reverse off, special feature of SSD1803            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=1 3-Line LCD (N=0)
-              break;  
+                    _function = 0x00;     //  Set function 0 0 1 DL N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=0 1 Line / 3 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x02;   // Set function, 0 0 1 DL N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=0 1 Line / 3 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803
+                    //   REV=0 Reverse off, special feature of SSD1803
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=1 3-Line LCD (N=0)
+                    break;
 
 //            case LCD10x2D:          // Special mode for SSD1803, 4-line mode but switch to double height font
-            case LCD10x4D:          // Special mode for SSD1803
-            case LCD20x4D:          // Special mode for SSD1803
-              _function = 0x08;     //  Set function 0 0 1 DL N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=1 4 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=1 4 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803
-                                    //   REV=0 Reverse off, special feature of SSD1803            
-                        
-              _lines = 0x01;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=1 4-Line LCD (N=1)
-              break;  
-
-            case LCD16x3G:          // Special mode for ST7036            
-            case LCD24x4D:          // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)       
-              _function = 0x08;     //  Set function 0 0 1 DL N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=1 2 line / 4 Line
-                                    //    DH=0 Double Height disable 
-                                    //    RE=0
-                                    //    IS=0
-          
-              _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 4-bit Databus,
-                                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
-                                    //     N=1 2 line / 4 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803
-                                    //    RE=1
-                                    //   REV=0 Reverse off, special feature of SSD1803            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=0 2-Line LCD (N=1)
-              break;                
-          } // switch type
-
-
-          // init special features 
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          
-          _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
-//          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)          
-          wait_ms(5);                               // Wait to ensure completion or SSD1803 fails to set Top/Bottom after reset..
-         
-          _writeCommand(0x08 | _lines);             // Set ext function 0 0 0 0 1 FW BW NW 1,2,3 or 4 lines (Ext Instr Set)
-
-          _writeCommand(0x10);                      // Double Height and Bias, 0 0 0 1 UD2=0, UD1=0, BS1=0 Bias 1/5, DH=0 (Ext Instr Set)
+                case LCD10x4D:          // Special mode for SSD1803
+                case LCD20x4D:          // Special mode for SSD1803
+                    _function = 0x08;     //  Set function 0 0 1 DL N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=1 4 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=1 4 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803
+                    //   REV=0 Reverse off, special feature of SSD1803
+
+                    _lines = 0x01;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=1 4-Line LCD (N=1)
+                    break;
+
+                case LCD16x3G:          // Special mode for ST7036
+                case LCD24x4D:          // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function = 0x08;     //  Set function 0 0 1 DL N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=1 2 line / 4 Line
+                    //    DH=0 Double Height disable
+                    //    RE=0
+                    //    IS=0
+
+                    _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 4-bit Databus,
+                    //         Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //     N=1 2 line / 4 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803
+                    //    RE=1
+                    //   REV=0 Reverse off, special feature of SSD1803
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=0 2-Line LCD (N=1)
+                    break;
+            } // switch type
+
+
+            // init special features
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV
+            // Select Extended Instruction Set
+
+            _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
+//          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)
+            wait_ms(5);                               // Wait to ensure completion or SSD1803 fails to set Top/Bottom after reset..
+
+            _writeCommand(0x08 | _lines);             // Set ext function 0 0 0 0 1 FW BW NW 1,2,3 or 4 lines (Ext Instr Set)
+
+            _writeCommand(0x10);                      // Double Height and Bias, 0 0 0 1 UD2=0, UD1=0, BS1=0 Bias 1/5, DH=0 (Ext Instr Set)
 
 //          _writeCommand(0x76);                      // Set TC Control, 0 1 1 1 0 1 1 0 (Ext Instr Set)
 //          _writeData(0x02);                         // Set TC data,    0 0 0 0 0 TC2,TC1,TC0 = 0 1 0 (Ext Instr Set)
 
-          _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS=1 Select Instruction Set 1
-                                                    // Select Std Instr set, Select IS=1  
-
-          _contrast = LCD_SSD1_CONTRAST;
-          _writeCommand(0x70 | (_contrast & 0x0F)); // Set Contrast 0 1 1 1 C3, C2, C1, C0 (Instr Set 1)
-                           
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS=1 Select Instruction Set 1
+            // Select Std Instr set, Select IS=1
+
+            _contrast = LCD_SSD1_CONTRAST;
+            _writeCommand(0x70 | (_contrast & 0x0F)); // Set Contrast 0 1 1 1 C3, C2, C1, C0 (Instr Set 1)
+
 //          _icon_power = 0x04;                       // Icon off, Booster on (Instr Set 1)
-          _icon_power = 0x0C;                       // Icon on, Booster on (Instr Set 1)          
-                                                    // Saved to allow contrast change at later time
-          _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));   // Set Power, Icon and Contrast, 0 1 0 1 Ion Bon C5 C4 (Instr Set 1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-
-          _writeCommand(0x68 | (LCD_SSD1_RAB & 0x07));  // Set Voltagefollower 0 1 1 0 Don = 1, Ampl ratio Rab2, Rab1, Rab0 = 1 1 0  (Instr Set 1)
-          wait_ms(10);            // Wait 10ms to ensure powered up
-
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV 
-                                                    // Select Extended Instruction Set 1
-          _writeCommand(0x10);                      // Shift/Scroll enable, 0 0 0 1 DS4/HS4 DS3/HS3 DS2/HS2 DS1/HS1  (Ext Instr Set 1)
-
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0
-         
-          break; // case SSD1803 Controller
-
-          
-      // Note1: The PCF21XX family of controllers has several types that dont have an onboard voltage generator for V-LCD.
-      //        You must supply this LCD voltage externally and not try to enable VGen. 
-      // Note2: The early versions of PCF2116 controllers (eg PCF2116C) can not generate sufficiently negative voltage for the LCD at a VDD of 3V3. 
-      //        You must supply this voltage externally and not enable VGen or you must use a higher VDD (e.g. 5V) and enable VGen.
-      //        More recent versions of the controller (eg PCF2116K) have an improved VGen that will work with 3V3.
-      // Note3: See datasheet, PCF2116 and other types provide a V0 pin to control the LCD contrast voltage that is provided by VGen. This pins allows 
-      //        contrast control similar to that of pin 3 on the standard 14pin LCD module connector.
-      //        You can disable VGen by connecting Vo to VDD. VLCD will then be used directly as LCD voltage.
-      // Note4: PCF2113 and PCF2119 are different wrt to VLCD generator! There is no V0 pin. The contrast voltage is software controlled by setting the VA and VB registers.
-      //        Vgen is automatically switched off when the contrast voltage VA or VB is set to 0x00. Note that certain limits apply to allowed values for VA and VB.                    
-      // Note5: See datasheet, members of the PCF21XX family support different numbers of rows/columns. Not all can support 3 or 4 rows.
-      // Note6: See datasheet, the PCF21XX-C and PCF21XX-K use a non-standard character set. This may result is strange looking text when not corrected..
-
-      case PCF2103_3V3:
-          // PCF2103 controller: No Voltage generator for VLCD, VDD=3V3..5V, VLCD input controls contrast voltage.                 
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD24x1:                    
-              _function = 0x00;       //FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=0 1-line/24 chars display mode, 0, H=0 
-                                      //Note: 4 bit mode is ignored for I2C mode
-              break;  
+            _icon_power = 0x0C;                       // Icon on, Booster on (Instr Set 1)
+            // Saved to allow contrast change at later time
+            _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03));   // Set Power, Icon and Contrast, 0 1 0 1 Ion Bon C5 C4 (Instr Set 1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x68 | (LCD_SSD1_RAB & 0x07));  // Set Voltagefollower 0 1 1 0 Don = 1, Ampl ratio Rab2, Rab1, Rab0 = 1 1 0  (Instr Set 1)
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV
+            // Select Extended Instruction Set 1
+            _writeCommand(0x10);                      // Shift/Scroll enable, 0 0 0 1 DS4/HS4 DS3/HS3 DS2/HS2 DS1/HS1  (Ext Instr Set 1)
+
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+
+            break; // case SSD1803 Controller
+
+
+            // Note1: The PCF21XX family of controllers has several types that dont have an onboard voltage generator for V-LCD.
+            //        You must supply this LCD voltage externally and not try to enable VGen.
+            // Note2: The early versions of PCF2116 controllers (eg PCF2116C) can not generate sufficiently negative voltage for the LCD at a VDD of 3V3.
+            //        You must supply this voltage externally and not enable VGen or you must use a higher VDD (e.g. 5V) and enable VGen.
+            //        More recent versions of the controller (eg PCF2116K) have an improved VGen that will work with 3V3.
+            // Note3: See datasheet, PCF2116 and other types provide a V0 pin to control the LCD contrast voltage that is provided by VGen. This pins allows
+            //        contrast control similar to that of pin 3 on the standard 14pin LCD module connector.
+            //        You can disable VGen by connecting Vo to VDD. VLCD will then be used directly as LCD voltage.
+            // Note4: PCF2113 and PCF2119 are different wrt to VLCD generator! There is no V0 pin. The contrast voltage is software controlled by setting the VA and VB registers.
+            //        Vgen is automatically switched off when the contrast voltage VA or VB is set to 0x00. Note that certain limits apply to allowed values for VA and VB.
+            // Note5: See datasheet, members of the PCF21XX family support different numbers of rows/columns. Not all can support 3 or 4 rows.
+            // Note6: See datasheet, the PCF21XX-C and PCF21XX-K use a non-standard character set. This may result is strange looking text when not corrected..
+
+        case PCF2103_3V3:
+            // PCF2103 controller: No Voltage generator for VLCD, VDD=3V3..5V, VLCD input controls contrast voltage.
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD24x1:
+                    _function = 0x00;       //FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=0 1-line/24 chars display mode, 0, H=0
+                    //Note: 4 bit mode is ignored for I2C mode
+                    break;
 
 //            case LCD12x1D:            //Special mode for PCF21XX, Only top line used
-            case LCD12x2:
-              _function = 0x04;       //FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=1 2-line/12 chars display mode, 0, H=0
-                                      //Note: 4 bit mode is ignored for I2C mode
-              break;  
-              
-            default:
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-            
-          } // switch type    
-
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-          wait_ms(10);            // Wait 10ms to ensure powered up                                                    
+                case LCD12x2:
+                    _function = 0x04;       //FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=1 2-line/12 chars display mode, 0, H=0
+                    //Note: 4 bit mode is ignored for I2C mode
+                    break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+            wait_ms(10);            // Wait 10ms to ensure powered up
 
 // Note: Display from GA628 shows 12 chars. This is actually the right half of a 24x1 display. The commons have been connected in reverse order.
-          _writeCommand(0x05);                             // Display Conf Set         0000 0, 1, P=0, Q=1               (Instr. Set 1)
-                                                        
-          _writeCommand(0x02);                             // Screen Config            0000 001, L=0  (Instr. Set 1)
-          _writeCommand(0x08);                             // ICON Conf                0000 1, IM=0 (Char mode), IB=0 (no Icon blink), 0 (Instr. Set 1) 
-
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-          
+            _writeCommand(0x05);                             // Display Conf Set         0000 0, 1, P=0, Q=1               (Instr. Set 1)
+
+            _writeCommand(0x02);                             // Screen Config            0000 001, L=0  (Instr. Set 1)
+            _writeCommand(0x08);                             // ICON Conf                0000 1, IM=0 (Char mode), IB=0 (no Icon blink), 0 (Instr. Set 1)
+
+            _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+
 #if(0)
-          // Select CG RAM
-          _writeCommand(0x40); //Set CG-RAM address, 8 sequential locations needed per UDC
-          // Store UDC/Icon pattern: 
-          //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
-          //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6) 
-          for (int i=0; i<(8 * 8); i++) {
+            // Select CG RAM
+            _writeCommand(0x40); //Set CG-RAM address, 8 sequential locations needed per UDC
+            // Store UDC/Icon pattern:
+            //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
+            //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6)
+            for (int i=0; i<(8 * 8); i++) {
 //            _writeData(0x1F);  // All On
-            _writeData(0x00);  // All Off            
-          }
+                _writeData(0x00);  // All Off
+            }
 #endif
-          break; // case PCF2103_3V3 Controller
-
-      case PCF2113_3V3:
-          // PCF2113 controller: Initialise Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
-          // Initialise Display configuration
-          switch (_type) {
-//            case LCD12x1:                                
+            break; // case PCF2103_3V3 Controller
+
+        case PCF2113_3V3:
+            // PCF2113 controller: Initialise Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
+            // Initialise Display configuration
+            switch (_type) {
+//            case LCD12x1:
 //              _function = 0x02;       // FUNCTION SET 0 0 1 DL=0 4 bit, 0, M=0 1-line/12 chars display mode, SL=1, IS=0
-                                      // Note: 4 bit mode is ignored for I2C mode
-            case LCD24x1:                    
-              _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 4 bit, 0, M=0 1-line/24 chars display mode, SL=0, IS=0            
-                                      // Note: 4 bit mode is ignored for I2C mode
-              break;  
-
-            case LCD12x2:                    
-              _function = 0x04;       // FUNCTION SET 0 0 1 DL=0 4 bit, 0, M=1 2-line/12 chars display mode, SL=0, IS=0            
-              break;  
-             
-            default:
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-                         
-          } // switch type    
-
-          // Init special features
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-
-          _writeCommand(0x04);                             // Display Conf Set         0000 0, 1, P=0, Q=0               (Instr. Set 1)
-          _writeCommand(0x10);                             // Temp Compensation Set    0001 0, 0, TC1=0, TC2=0           (Instr. Set 1)
+                    // Note: 4 bit mode is ignored for I2C mode
+                case LCD24x1:
+                    _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 4 bit, 0, M=0 1-line/24 chars display mode, SL=0, IS=0
+                    // Note: 4 bit mode is ignored for I2C mode
+                    break;
+
+                case LCD12x2:
+                    _function = 0x04;       // FUNCTION SET 0 0 1 DL=0 4 bit, 0, M=1 2-line/12 chars display mode, SL=0, IS=0
+                    break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            // Init special features
+            _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+
+            _writeCommand(0x04);                             // Display Conf Set         0000 0, 1, P=0, Q=0               (Instr. Set 1)
+            _writeCommand(0x10);                             // Temp Compensation Set    0001 0, 0, TC1=0, TC2=0           (Instr. Set 1)
 //          _writeCommand(0x42);                             // HV GEN                   0100 S1=1, S2=0 (2x multiplier)   (Instr. Set 1)
-          _writeCommand(0x40 | (LCD_PCF2_S12 & 0x03));     // HV Gen                   0100 S1=1, S2=0 (2x multiplier)   (Instr. Set 1)
-          
-          _contrast = LCD_PCF2_CONTRAST;              
-          _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)  1, V=0, VA=contrast
-          _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)  1, V=1, VB=contrast
-          wait_ms(10);            // Wait 10ms to ensure powered up
-          
-          _writeCommand(0x02);                             // Screen Config            0000 001, L=0  (Instr. Set 1)
-          _writeCommand(0x08);                             // ICON Conf                0000 1, IM=0 (Char mode), IB=0 (no icon blink) DM=0 (no direct mode) (Instr. Set 1) 
-
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-
-          break; // case PCF2113_3V3 Controller
+            _writeCommand(0x40 | (LCD_PCF2_S12 & 0x03));     // HV Gen                   0100 S1=1, S2=0 (2x multiplier)   (Instr. Set 1)
+
+            _contrast = LCD_PCF2_CONTRAST;
+            _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)  1, V=0, VA=contrast
+            _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)  1, V=1, VB=contrast
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x02);                             // Screen Config            0000 001, L=0  (Instr. Set 1)
+            _writeCommand(0x08);                             // ICON Conf                0000 1, IM=0 (Char mode), IB=0 (no icon blink) DM=0 (no direct mode) (Instr. Set 1)
+
+            _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+
+            break; // case PCF2113_3V3 Controller
 
 
 //      case PCF2113_5V:
-          // PCF2113 controller: No Voltage generator for VLCD. VDD=5V. Contrast voltage controlled by VA or VB.
-//@TODO                            
-
-
-      case PCF2116_3V3:
-          // PCF2116 controller: Voltage generator for VLCD. VDD=5V. V0 controls contrast voltage.                 
-          // Initialise Display configuration
-          switch (_type) {
+            // PCF2113 controller: No Voltage generator for VLCD. VDD=5V. Contrast voltage controlled by VA or VB.
+//@TODO
+
+
+        case PCF2116_3V3:
+            // PCF2116 controller: Voltage generator for VLCD. VDD=5V. V0 controls contrast voltage.
+            // Initialise Display configuration
+            switch (_type) {
 //            case LCD12x1:
-//            case LCD12x2:                                                                            
-            case LCD24x1:                    
-              _writeCommand(0x22);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=0/M=0 1-line/24 chars display mode, G=1 Vgen on, 0 
-                                      //Note: 4 bit mode is ignored for I2C mode
-              wait_ms(10);            // Wait 10ms to ensure powered up                                                    
-              break;  
-
-            case LCD12x3D:            // Special mode for KS0078 and PCF21XX                            
-            case LCD12x3D1:           // Special mode for PCF21XX                     
-            case LCD12x4D:            // Special mode for PCF21XX:
-              _writeCommand(0x2E);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=1 4-line/12 chars display mode, G=1 VGen on, 0                               
-                                      //Note: 4 bit mode is ignored for I2C mode              
-              wait_ms(10);            // Wait 10ms to ensure powered up                                                    
-              break;  
-
-            case LCD24x2:
-              _writeCommand(0x2A);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=0 2-line/24 chars display mode, G=1 VGen on, 0
-                                      //Note: 4 bit mode is ignored for I2C mode
-              wait_ms(10);            // Wait 10ms to ensure powered up   
-              break;  
-              
-            default:
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-            
-          } // switch type    
-
-          break; // case PCF2116_3V3 Controller
+//            case LCD12x2:
+                case LCD24x1:
+                    _writeCommand(0x22);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=0/M=0 1-line/24 chars display mode, G=1 Vgen on, 0
+                    //Note: 4 bit mode is ignored for I2C mode
+                    wait_ms(10);            // Wait 10ms to ensure powered up
+                    break;
+
+                case LCD12x3D:            // Special mode for KS0078 and PCF21XX
+                case LCD12x3D1:           // Special mode for PCF21XX
+                case LCD12x4D:            // Special mode for PCF21XX:
+                    _writeCommand(0x2E);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=1 4-line/12 chars display mode, G=1 VGen on, 0
+                    //Note: 4 bit mode is ignored for I2C mode
+                    wait_ms(10);            // Wait 10ms to ensure powered up
+                    break;
+
+                case LCD24x2:
+                    _writeCommand(0x2A);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=0 2-line/24 chars display mode, G=1 VGen on, 0
+                    //Note: 4 bit mode is ignored for I2C mode
+                    wait_ms(10);            // Wait 10ms to ensure powered up
+                    break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            break; // case PCF2116_3V3 Controller
 
 
 //Experimental for cellphone 3-line display, SA=0x74, No Ack supported, Character set C or K, DL = 8 bit, N=0,M=1 (reserved mode !!), external VLCD -2V5
-//@TODO                            
-      case PCF2116_5V:
-          // PCF2116 controller: No Voltage generator for VLCD. VDD=5V. V0 controls contrast voltage.                           
-          // Initialise Display configuration
-          switch (_type) {
+//@TODO
+        case PCF2116_5V:
+            // PCF2116 controller: No Voltage generator for VLCD. VDD=5V. V0 controls contrast voltage.
+            // Initialise Display configuration
+            switch (_type) {
 //            case LCD12x1:
-//            case LCD12x2:                                                                            
-//            case LCD24x1:                    
-//              _writeCommand(0x20);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=0/M=0 1-line/24 chars display mode, G=0 no Vgen, 0 
-                                      //Note: 4 bit mode is ignored for I2C mode
-//              wait_ms(10);            // Wait 10ms to ensure powered up                                                    
-//              break;  
-
-            case LCD12x3D:            // Special mode for KS0078 and PCF21XX                            
-            case LCD12x3D1:           // Special mode for PCF21XX                     
-            case LCD12x4D:            // Special mode for PCF21XX:
+//            case LCD12x2:
+//            case LCD24x1:
+//              _writeCommand(0x20);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=0/M=0 1-line/24 chars display mode, G=0 no Vgen, 0
+                    //Note: 4 bit mode is ignored for I2C mode
+//              wait_ms(10);            // Wait 10ms to ensure powered up
+//              break;
+
+                case LCD12x3D:            // Special mode for KS0078 and PCF21XX
+                case LCD12x3D1:           // Special mode for PCF21XX
+                case LCD12x4D:            // Special mode for PCF21XX:
 //              _writeCommand(0x34);    //FUNCTION SET 8 bit, N=0/M=1 4-line/12 chars display mode      OK
-//              _writeCommand(0x24);    //FUNCTION SET 4 bit, N=0/M=1 4-line/12 chars display mode      OK                                            
-              _writeCommand(0x2C);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=1 4-line/12 chars display mode, G=0 no Vgen, 0  OK       
-                                      //Note: 4 bit mode is ignored for I2C mode              
-              wait_ms(10);            // Wait 10ms to ensure powered up                                                    
-              break;  
+//              _writeCommand(0x24);    //FUNCTION SET 4 bit, N=0/M=1 4-line/12 chars display mode      OK
+                    _writeCommand(0x2C);    //FUNCTION SET 0 0 1 DL=0 4-bit, N=1/M=1 4-line/12 chars display mode, G=0 no Vgen, 0  OK
+                    //Note: 4 bit mode is ignored for I2C mode
+                    wait_ms(10);            // Wait 10ms to ensure powered up
+                    break;
 
 //            case LCD24x2:
 //              _writeCommand(0x28);    //FUNCTION SET 4 bit, N=1/M=0 2-line/24 chars display mode
-                                      //Note: 4 bit mode is ignored for I2C mode
-//              wait_ms(10);            // Wait 10ms to ensure powered up   
-//              break;  
-              
-            default:
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-            
-          } // switch type    
-
-          break; // case PCF2116_5V Controller
-
-      case PCF2119_3V3:
-      case PCF2119R_3V3:
-          // PCF2119 controller: Initialise Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
-          // Note1: See datasheet, the PCF2119 supports icons and provides separate constrast control for Icons and characters.
-          // Note2: Vgen is switched off when the contrast voltage VA or VB is set to 0x00.
-                  
+                    //Note: 4 bit mode is ignored for I2C mode
+//              wait_ms(10);            // Wait 10ms to ensure powered up
+//              break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            break; // case PCF2116_5V Controller
+
+        case PCF2119_3V3:
+        case PCF2119R_3V3:
+            // PCF2119 controller: Initialise Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
+            // Note1: See datasheet, the PCF2119 supports icons and provides separate constrast control for Icons and characters.
+            // Note2: Vgen is switched off when the contrast voltage VA or VB is set to 0x00.
+
 //POR or Hardware Reset should be applied
-          wait_ms(10);            // Wait 10ms to ensure powered up   
-
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:
 //            case LCD12x1:
-            case LCD16x1:           
-              _function = 0x02;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0 , M=0 1-line/16 chars display mode, SL=1
-                                      // Note: 4 bit mode is ignored for I2C mode
-              break;  
-            
-            case LCD24x1:                    
-//            case LCD32x1:                                
-              _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0 , M=0 1-line/32 chars display mode, SL=0
-                                      // Note: 4 bit mode is ignored for I2C mode
-              break;  
-
-            case LCD8x2:
-//            case LCD12x2:            
-            case LCD16x2:
-              _function = 0x04;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=1 2-line/16 chars display mode, SL=0
-                                      // Note: 4 bit mode is ignored for I2C mode
-              break;  
-             
-            default:
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-            
-          } // switch type    
-
-          // Init special features 
-          _writeCommand(0x20 | _function | 0x01);           // Set function, Select Instruction Set = 1              
+                case LCD16x1:
+                    _function = 0x02;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0 , M=0 1-line/16 chars display mode, SL=1
+                    // Note: 4 bit mode is ignored for I2C mode
+                    break;
+
+                case LCD24x1:
+//            case LCD32x1:
+                    _function = 0x00;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0 , M=0 1-line/32 chars display mode, SL=0
+                    // Note: 4 bit mode is ignored for I2C mode
+                    break;
+
+                case LCD8x2:
+//            case LCD12x2:
+                case LCD16x2:
+                    _function = 0x04;       // FUNCTION SET 0 0 1 DL=0 4-bit, 0, M=1 2-line/16 chars display mode, SL=0
+                    // Note: 4 bit mode is ignored for I2C mode
+                    break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            // Init special features
+            _writeCommand(0x20 | _function | 0x01);           // Set function, Select Instruction Set = 1
 
 //          _writeCommand(0x04);    // DISP CONF SET (Instr. Set 1)   0000, 0, 1, P=0, Q=0 (IC at Bottom)
 //          _writeCommand(0x05);    // Display Conf Set               0000, 0, 1, P=0, Q=1
 //          _writeCommand(0x06);    // Display Conf Set               0000, 0, 1, P=1, Q=0
-          _writeCommand(0x07);    // Display Conf Set               0000, 0, 1, P=1, Q=1    (IC at Top)
-
-          _writeCommand(0x10);    // TEMP CTRL SET (Instr. Set 1)   0001, 0, 0, TC1=0, TC2=0
+            _writeCommand(0x07);    // Display Conf Set               0000, 0, 1, P=1, Q=1    (IC at Top)
+
+            _writeCommand(0x10);    // TEMP CTRL SET (Instr. Set 1)   0001, 0, 0, TC1=0, TC2=0
 //          _writeCommand(0x42);    // HV GEN (Instr. Set 1)          0100, 0, 0, S1=1, S2=0 (2x multiplier)
-          _writeCommand(0x40 | (LCD_PCF2_S12 & 0x03));      // HV GEN (Instr. Set 1)          0100, 0, 0, S1=1, S2=0 (2x multiplier)
-
-          _contrast = LCD_PCF2_CONTRAST;              
-          _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=0, VA=contrast
-          _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=1, VB=contrast
-          wait_ms(10);            // Wait 10ms to ensure powered up
-          
-          _writeCommand(0x02);    // SCRN CONF (Instr. Set 1)    L=0
-          _writeCommand(0x08);    // ICON CONF (Instr. Set 1)    IM=0 (Char mode) IB=0 (no icon blink) DM=0 (no direct mode)
-
-          _writeCommand(0x20 | _function);                  // Select Instruction Set = 0
-
-          break; // case PCF2119_3V3 Controller
+            _writeCommand(0x40 | (LCD_PCF2_S12 & 0x03));      // HV GEN (Instr. Set 1)          0100, 0, 0, S1=1, S2=0 (2x multiplier)
+
+            _contrast = LCD_PCF2_CONTRAST;
+            _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=0, VA=contrast
+            _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=1, VB=contrast
+            wait_ms(10);            // Wait 10ms to ensure powered up
+
+            _writeCommand(0x02);    // SCRN CONF (Instr. Set 1)    L=0
+            _writeCommand(0x08);    // ICON CONF (Instr. Set 1)    IM=0 (Char mode) IB=0 (no icon blink) DM=0 (no direct mode)
+
+            _writeCommand(0x20 | _function);                  // Select Instruction Set = 0
+
+            break; // case PCF2119_3V3 Controller
 
 //      case PCF2119_5V:
-          // PCF2119 controller: No Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
-          // Note1: See datasheet, the PCF2119 supports icons and provides separate constrast control for Icons and characters.
-          // Note2: Vgen is switched off when the contrast voltage VA or VB is set to 0x00.                     
-//@TODO                            
-
-      case WS0010:         
-          // WS0010 OLED controller: Initialise DC/DC Voltage converter for LEDs
-          // Note1: Identical to RS0010  
-          // Note2: supports 1 or 2 lines (and 16x100 graphics)
-          //        supports 4 fonts (English/Japanese (default), Western European-I, English/Russian, Western European-II)
-                           // Cursor/Disp shift set 0001 SC RL  0 0
-                           //
-                           // Mode and Power set    0001 GC PWR 1 1                           
-                           //  GC  = 0 (Graph Mode=1, Char Mode=0)             
-                           //  PWR = 1 (DC/DC On/Off)
-   
+            // PCF2119 controller: No Voltage booster for VLCD. VDD=3V3. VA and VB control contrast.
+            // Note1: See datasheet, the PCF2119 supports icons and provides separate constrast control for Icons and characters.
+            // Note2: Vgen is switched off when the contrast voltage VA or VB is set to 0x00.
+//@TODO
+
+        case WS0010:
+            // WS0010 OLED controller: Initialise DC/DC Voltage converter for LEDs
+            // Note1: Identical to RS0010
+            // Note2: supports 1 or 2 lines (and 16x100 graphics)
+            //        supports 4 fonts (English/Japanese (default), Western European-I, English/Russian, Western European-II)
+            // Cursor/Disp shift set 0001 SC RL  0 0
+            //
+            // Mode and Power set    0001 GC PWR 1 1
+            //  GC  = 0 (Graph Mode=1, Char Mode=0)
+            //  PWR = 1 (DC/DC On/Off)
+
 //@Todo: This may be needed to enable a warm reboot
-          //_writeCommand(0x13);   // Char mode, DC/DC off              
-          //wait_ms(10);           // Wait 10ms to ensure powered down                  
-          _writeCommand(0x17);   // Char mode, DC/DC on        
-          wait_ms(10);           // Wait 10ms to ensure powered up        
-
-          // Initialise Display configuration
-          switch (_type) {                    
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2B is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:                                            
-            case LCD24x1:
-              _writeCommand(0x20); // Function set 001 DL N F FT1 FT0
-                                   //  DL=0  (4 bits bus)             
-                                   //   N=0  (1 line)
-                                   //   F=0  (5x7 dots font)
-                                   //  FT=00 (00 = Engl/Jap, 01 = WestEur1, 10 = Engl/Russian, 11 = WestEur2
-              break;  
-
-            case LCD12x3D:            // Special mode for KS0078 and PCF21XX                            
-            case LCD12x3D1:           // Special mode for PCF21XX                     
-            case LCD12x4D:            // Special mode for PCF21XX:
-            case LCD16x3G:            // Special mode for ST7036            
-            case LCD24x4D:            // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)       
-              _writeCommand(0x28); // Function set 001 DL N F FT1 FT0
-                                   //  DL=0  (4 bits bus)
-                                   //   N=1  (2 lines)
-                                   //   F=0  (5x7 dots font)
-                                   //  FT=00 (00 = Engl/Jap, 01 = WestEur1, 10 = Engl/Russian, 11 = WestEur2
-
-              break;
-           } // switch type
-           
-           break; // case WS0010 Controller
-
-
-      case US2066_3V3:
-          // US2066/SSD1311 OLED controller, Initialise for VDD=3V3
-          // Note: supports 1,2, 3 or 4 lines
+            //_writeCommand(0x13);   // Char mode, DC/DC off
+            //wait_ms(10);           // Wait 10ms to ensure powered down
+            _writeCommand(0x17);   // Char mode, DC/DC on
+            wait_ms(10);           // Wait 10ms to ensure powered up
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2B is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+                case LCD24x1:
+                    _writeCommand(0x20); // Function set 001 DL N F FT1 FT0
+                    //  DL=0  (4 bits bus)
+                    //   N=0  (1 line)
+                    //   F=0  (5x7 dots font)
+                    //  FT=00 (00 = Engl/Jap, 01 = WestEur1, 10 = Engl/Russian, 11 = WestEur2
+                    break;
+
+                case LCD12x3D:            // Special mode for KS0078 and PCF21XX
+                case LCD12x3D1:           // Special mode for PCF21XX
+                case LCD12x4D:            // Special mode for PCF21XX:
+                case LCD16x3G:            // Special mode for ST7036
+                case LCD24x4D:            // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _writeCommand(0x28); // Function set 001 DL N F FT1 FT0
+                    //  DL=0  (4 bits bus)
+                    //   N=1  (2 lines)
+                    //   F=0  (5x7 dots font)
+                    //  FT=00 (00 = Engl/Jap, 01 = WestEur1, 10 = Engl/Russian, 11 = WestEur2
+
+                    break;
+            } // switch type
+
+            break; // case WS0010 Controller
+
+
+        case US2066_3V3:
+            // US2066/SSD1311 OLED controller, Initialise for VDD=3V3
+            // Note: supports 1,2, 3 or 4 lines
 //      case USS2066_5V:
-          // US2066 controller, VDD=5V
-                    
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2D is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:   
-//            case LCD20x1:                                                                         
-              _function = 0x00;     //  Set function 0 0 1 X N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
-                                    //     N=0 1 Line / 3 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x02;   // Set function, 0 0 1 X N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead                                    
-                                    //     N=0 1 Line / 3 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
-                                    //   REV=0 Reverse off, special feature of SSD1803, US2066            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=0 1-Line LCD (N=0)
-              break;  
-
-            case LCD16x1C:
-            case LCD8x2:
-            case LCD16x2:
-            case LCD20x2:            
-              _function = 0x08;     //  Set function 0 0 1 X N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead                                                                        
-                                    //     N=1 2 line / 4 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x0A;   // Set function, 0 0 1 X N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead                                                                        
-                                    //     N=1 2 line / 4 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
-                                    //   REV=0 Reverse off, special feature of SSD1803, US2066            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=0 2-Line LCD (N=1)
-              break;                
-
-            case LCD12x3D:          // Special mode for KS0078 and PCF21XX 
-//            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX            
-            case LCD16x3D:          // Special mode for KS0078, SSD1803 and US2066
+            // US2066 controller, VDD=5V
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2D is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+//            case LCD20x1:
+                    _function = 0x00;     //  Set function 0 0 1 X N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=0 1 Line / 3 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x02;   // Set function, 0 0 1 X N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=0 1 Line / 3 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
+                    //   REV=0 Reverse off, special feature of SSD1803, US2066
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=0 1-Line LCD (N=0)
+                    break;
+
+                case LCD16x1C:
+                case LCD8x2:
+                case LCD16x2:
+                case LCD20x2:
+                    _function = 0x08;     //  Set function 0 0 1 X N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=1 2 line / 4 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x0A;   // Set function, 0 0 1 X N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=1 2 line / 4 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
+                    //   REV=0 Reverse off, special feature of SSD1803, US2066
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=0 2-Line LCD (N=1)
+                    break;
+
+                case LCD12x3D:          // Special mode for KS0078 and PCF21XX
+//            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
+                case LCD16x3D:          // Special mode for KS0078, SSD1803 and US2066
 //            case LCD16x3D1:           // Special mode for SSD1803, US2066
 //            case LCD20x3D:            // Special mode for SSD1803, US2066
-              _function = 0x00;     //  Set function 0 0 1 X N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead                                    
-                                    //     N=0 1 Line / 3 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x02;   // Set function, 0 0 1 X N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead                                    
-                                    //     N=0 1 Line / 3 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
-                                    //   REV=0 Reverse off, special feature of SSD1803, US2066            
-                        
-              _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=1 3-Line LCD (N=0)
-              break;  
-
-            case LCD20x4D:          // Special mode for SSD1803, US2066
-              _function = 0x08;     //  Set function 0 0 1 X N DH RE(0) IS 
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
-                                    //     N=1 2 line / 4 Line
-                                    //    DH=0 Double Height disable 
-                                    //    IS=0
-          
-              _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
-                                    //  Saved to allow switch between Instruction sets at later time
-                                    //    DL=0 bit is ignored for US2066. Uses hardwired pins instead                                    
-                                    //     N=1 2 line / 4 Line
-                                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
-                                    //   REV=0 Reverse off, special feature of SSD1803, US2066            
-                        
-              _lines = 0x01;        // Ext function set 0 0 0 0 1 FW BW NW 
-                                    //    NW=1 4-Line LCD (N=1)
-              break;  
-
-//            case LCD24x1:                                                                         
-//            case LCD16x3G:          // Special mode for ST7036            
+                    _function = 0x00;     //  Set function 0 0 1 X N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=0 1 Line / 3 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x02;   // Set function, 0 0 1 X N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=0 1 Line / 3 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
+                    //   REV=0 Reverse off, special feature of SSD1803, US2066
+
+                    _lines = 0x00;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=1 3-Line LCD (N=0)
+                    break;
+
+                case LCD20x4D:          // Special mode for SSD1803, US2066
+                    _function = 0x08;     //  Set function 0 0 1 X N DH RE(0) IS
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=X bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=1 2 line / 4 Line
+                    //    DH=0 Double Height disable
+                    //    IS=0
+
+                    _function_1 = 0x0A;   // Set function, 0 0 1 DL N BE RE(1) REV
+                    //  Saved to allow switch between Instruction sets at later time
+                    //    DL=0 bit is ignored for US2066. Uses hardwired pins instead
+                    //     N=1 2 line / 4 Line
+                    //    BE=0 Blink Enable off, special feature of SSD1803, US2066
+                    //   REV=0 Reverse off, special feature of SSD1803, US2066
+
+                    _lines = 0x01;        // Ext function set 0 0 0 0 1 FW BW NW
+                    //    NW=1 4-Line LCD (N=1)
+                    break;
+
+//            case LCD24x1:
+//            case LCD16x3G:          // Special mode for ST7036
 //            case LCD24x4D:          // Special mode for KS0078
-            default:            
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-          } // switch type
-
-          _writeCommand(0x00);                      // NOP, make sure to sync SPI
-
-          // init special features 
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-
-          _writeCommand(0x71);                      // Function Select A: 0 1 1 1 0 0 0 1 (Ext Instr Set)
-          _writeData(0x00);                         // Disable Internal VDD
-
-          _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
-
-          _writeCommand(0xD5);                      // Display Clock Divide Ratio: 1 1 0 1 0 1 0 1 (Ext Instr Set, OLED Instr Set)
-          _writeCommand(0x70);                      // Display Clock Divide Ratio value: 0 1 1 1 0 0 0 0 (Ext Instr Set, OLED Instr Set)
-                    
-          _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)
-          
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+            } // switch type
+
+            _writeCommand(0x00);                      // NOP, make sure to sync SPI
+
+            // init special features
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+            // Select Extended Instruction Set
+
+            _writeCommand(0x71);                      // Function Select A: 0 1 1 1 0 0 0 1 (Ext Instr Set)
+            _writeData(0x00);                         // Disable Internal VDD
+
+            _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
+
+            _writeCommand(0xD5);                      // Display Clock Divide Ratio: 1 1 0 1 0 1 0 1 (Ext Instr Set, OLED Instr Set)
+            _writeCommand(0x70);                      // Display Clock Divide Ratio value: 0 1 1 1 0 0 0 0 (Ext Instr Set, OLED Instr Set)
+
+            _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)
+
 //          _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
-          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)          
-         
-          _writeCommand(0x08 | _lines);             // Set ext function 0 0 0 0 1 FW BW NW 1,2,3 or 4 lines (Ext Instr Set)
+            _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)
+
+            _writeCommand(0x08 | _lines);             // Set ext function 0 0 0 0 1 FW BW NW 1,2,3 or 4 lines (Ext Instr Set)
 
 //          _writeCommand(0x1C);                      // Double Height, 0 0 0 1 UD2=1, UD1=1, X, DH'=0 (Ext Instr Set)
 //                                                    // Default
 
-          _writeCommand(0x72);                      // Function Select B: 0 1 1 1 0 0 1 0 (Ext Instr Set)
-          _writeData(0x01);                         // Select ROM A (CGRAM 8, CGROM 248)
-
-          _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
-
-          _writeCommand(0xDA);                      // Set Segm Pins Config:  1 1 0 1 1 0 1 0 (Ext Instr Set, OLED)
-          _writeCommand(0x10);                      // Set Segm Pins Config value: Altern Odd/Even, Disable Remap (Ext Instr Set, OLED)
-
-          _writeCommand(0xDC);                      // Function Select C: 1 1 0 1 1 1 0 0 (Ext Instr Set, OLED)
+            _writeCommand(0x72);                      // Function Select B: 0 1 1 1 0 0 1 0 (Ext Instr Set)
+            _writeData(0x01);                         // Select ROM A (CGRAM 8, CGROM 248)
+
+            _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
+
+            _writeCommand(0xDA);                      // Set Segm Pins Config:  1 1 0 1 1 0 1 0 (Ext Instr Set, OLED)
+            _writeCommand(0x10);                      // Set Segm Pins Config value: Altern Odd/Even, Disable Remap (Ext Instr Set, OLED)
+
+            _writeCommand(0xDC);                      // Function Select C: 1 1 0 1 1 1 0 0 (Ext Instr Set, OLED)
 //          _writeCommand(0x00);                      // Set internal VSL, GPIO pin HiZ (always read low)
-          _writeCommand(0x80);                      // Set external VSL, GPIO pin HiZ (always read low)
-
-          _contrast = LCD_US20_CONTRAST;
-          _writeCommand(0x81);                      // Set Contrast Control: 1 0 0 0 0 0 0 1 (Ext Instr Set, OLED)
-          _writeCommand((_contrast << 2) | 0x03);   // Set Contrast Value: 8 bits, use 6 bits for compatibility 
-
-          _writeCommand(0xD9);                      // Set Phase Length: 1 1 0 1 1 0 0 1 (Ext Instr Set, OLED)
-          _writeCommand(0xF1);                      // Set Phase Length Value: 
-
-          _writeCommand(0xDB);                      // Set VCOMH Deselect Lvl: 1 1 0 1 1 0 1 1 (Ext Instr Set, OLED)
-          _writeCommand(0x30);                      // Set VCOMH Deselect Value: 0.83 x VCC
-
-          wait_ms(10);            // Wait 10ms to ensure powered up
+            _writeCommand(0x80);                      // Set external VSL, GPIO pin HiZ (always read low)
+
+            _contrast = LCD_US20_CONTRAST;
+            _writeCommand(0x81);                      // Set Contrast Control: 1 0 0 0 0 0 0 1 (Ext Instr Set, OLED)
+            _writeCommand((_contrast << 2) | 0x03);   // Set Contrast Value: 8 bits, use 6 bits for compatibility
+
+            _writeCommand(0xD9);                      // Set Phase Length: 1 1 0 1 1 0 0 1 (Ext Instr Set, OLED)
+            _writeCommand(0xF1);                      // Set Phase Length Value:
+
+            _writeCommand(0xDB);                      // Set VCOMH Deselect Lvl: 1 1 0 1 1 0 1 1 (Ext Instr Set, OLED)
+            _writeCommand(0x30);                      // Set VCOMH Deselect Value: 0.83 x VCC
+
+            wait_ms(10);            // Wait 10ms to ensure powered up
 
 //Test Fade/Blinking. Hard Blink on/off, No fade in/out ??
 //          _writeCommand(0x23);                      // Set (Ext Instr Set, OLED)
 //          _writeCommand(0x3F);                      // Set interval 128 frames
 //End Test Blinking
 
-          _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)
-          
-          _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 X N DH RE(0) IS=1 Select Instruction Set 1
-                                                    // Select Std Instr set, Select IS=1  
-
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Ext Instr Set, IS=1
-          _writeCommand(0x10);                      // Shift/Scroll enable, 0 0 0 1 DS4/HS4 DS3/HS3 DS2/HS2 DS1/HS1  (Ext Instr Set, IS=1)
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // case US2066/SSD1311 Controller
-
-      //not yet tested on hardware
-      case PT6314 :
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2B is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:                                            
-            case LCD20x1:                                                        
-            case LCD24x1:
-              _function = 0x00;    // Function set 001 DL N X BR1 BR0
-                                   //  DL=0 (4 bits bus)
-                                   //  Note: 4 bit mode is ignored for native SPI and I2C devices                                                                                 
-                                   //  N=0 (1 line)
-                                   //  X
-                                   //  BR1=0 (2 significant bits for brightness
-                                   //  BR0=0 
-                                   //           0x0 = 100%
-                                   //           0x1 =  75%
-                                   //           0x2 =  50%
-                                   //           0x3 =  25%                
-
-              break;                                
-                                                  
-            // All other valid LCD types are initialised as 2 Line displays
-            case LCD8x2:  
-            case LCD16x2:  
-            case LCD20x2:
-            case LCD24x2:
-              _function = 0x08;    // Function set 001 DL N X BR1 BR2
-                                   //  DL=0 (4 bits bus)
-                                   //  Note: 4 bit mode is ignored for native SPI and I2C devices                                 
-                                   //  N=1 (2 lines)
-                                   //  X
-                                   //  BR1=0 (2 significant bits for brightness
-                                   //  BR0=0 
-              break;
-              
-            default:            
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;               
-          } // switch type
-           
-          _contrast = LCD_PT63_CONTRAST;
-          _writeCommand(0x20 | _function | ((~_contrast) >> 4));        // Invert and shift to use 2 MSBs     
-          break; // case PT6314 Controller (VFD)
-
-
-      case HD66712:
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD12x1:                                
-            case LCD16x1:                                            
-            case LCD20x1:
-            case LCD24x1:
+            _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)
+
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 X N DH RE(0) IS=1 Select Instruction Set 1
+            // Select Std Instr set, Select IS=1
+
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+            // Select Ext Instr Set, IS=1
+            _writeCommand(0x10);                      // Shift/Scroll enable, 0 0 0 1 DS4/HS4 DS3/HS3 DS2/HS2 DS1/HS1  (Ext Instr Set, IS=1)
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+            break; // case US2066/SSD1311 Controller
+
+            //not yet tested on hardware
+        case PT6314 :
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2B is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+                case LCD20x1:
+                case LCD24x1:
+                    _function = 0x00;    // Function set 001 DL N X BR1 BR0
+                    //  DL=0 (4 bits bus)
+                    //  Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //  N=0 (1 line)
+                    //  X
+                    //  BR1=0 (2 significant bits for brightness
+                    //  BR0=0
+                    //           0x0 = 100%
+                    //           0x1 =  75%
+                    //           0x2 =  50%
+                    //           0x3 =  25%
+
+                    break;
+
+                    // All other valid LCD types are initialised as 2 Line displays
+                case LCD8x2:
+                case LCD16x2:
+                case LCD20x2:
+                case LCD24x2:
+                    _function = 0x08;    // Function set 001 DL N X BR1 BR2
+                    //  DL=0 (4 bits bus)
+                    //  Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //  N=1 (2 lines)
+                    //  X
+                    //  BR1=0 (2 significant bits for brightness
+                    //  BR0=0
+                    break;
+
+                default:
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+            } // switch type
+
+            _contrast = LCD_PT63_CONTRAST;
+            _writeCommand(0x20 | _function | ((~_contrast) >> 4));        // Invert and shift to use 2 MSBs
+            break; // case PT6314 Controller (VFD)
+
+
+        case HD66712:
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD12x1:
+                case LCD16x1:
+                case LCD20x1:
+                case LCD24x1:
 //            case LCD32x1:        // EXT pin is High, extension driver needed
-              _function  = 0x02;    // Function set 001 DL N RE(0) - - (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
-                                    //   
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=1  (Ena Extended Regs; special mode for HD66712)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM; special mode for HD66712)                                
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal; special mode for HD66712)
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)                                
-              break;                                
+                    _function  = 0x02;    // Function set 001 DL N RE(0) - - (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
+                    //
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=1  (Ena Extended Regs; special mode for HD66712)
+                    //   BE=0  (Blink Enable, CG/SEG RAM; special mode for HD66712)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal; special mode for HD66712)
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)
+                    break;
 
 //            case LCD12x3D:         // Special mode for KS0073, KS0078, PCF21XX and HD66712
 //            case LCD12x3D1:        // Special mode for KS0073, KS0078, PCF21XX and HD66712
-            case LCD12x4D:         // Special mode for KS0073, KS0078, PCF21XX and HD66712
+                case LCD12x4D:         // Special mode for KS0073, KS0078, PCF21XX and HD66712
 //            case LCD16x3D:         // Special mode for KS0073, KS0078 and HD66712
-//            case LCD16x4D:         // Special mode for KS0073, KS0078 and HD66712            
-            case LCD20x4D:         // Special mode for KS0073, KS0078 and HD66712            
-              _function  = 0x02;    // Function set 001 DL N RE(0) - - (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
-                                    //   
-                                    
-              _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=0  (1-line mode, N=1 2-line mode)
-                                    //   RE=1  (Ena Extended Regs; special mode for HD66712)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM; special mode for HD66712)                                
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal; special mode for HD66712)
-
-              _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)                                
-              break;                                
-
-            case LCD16x3G:            // Special mode for ST7036                        
+//            case LCD16x4D:         // Special mode for KS0073, KS0078 and HD66712
+                case LCD20x4D:         // Special mode for KS0073, KS0078 and HD66712
+                    _function  = 0x02;    // Function set 001 DL N RE(0) - - (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
+                    //
+
+                    _function_1 = 0x04;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=0  (1-line mode, N=1 2-line mode)
+                    //   RE=1  (Ena Extended Regs; special mode for HD66712)
+                    //   BE=0  (Blink Enable, CG/SEG RAM; special mode for HD66712)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal; special mode for HD66712)
+
+                    _function_x = 0x01;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //    NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)
+                    break;
+
+                case LCD16x3G:            // Special mode for ST7036
 //            case LCD24x3D:         // Special mode for KS0078
 //            case LCD24x3D1:        // Special mode for KS0078
-            case LCD24x4D:         // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            default:
-              // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)            
-              _function  = 0x0A;    // Function set 001 DL N RE(0) - - (Std Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (2-line mode), N=0 (1-line mode)
-                                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
-                                    
-              _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                    //   DL=0  (4 bits bus)             
-                                    //    N=1  (2 line mode), N=0 (1-line mode)
-                                    //   RE=1  (Ena Extended Regs, special mode for HD66712)
-                                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for HD66712)
-                                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
-
-              _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)
-              break;
-          } // switch type
-
-          // init special features
-          _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE LP (Ext Regs)
-                                           //   DL=0 (4 bits bus), DL=1 (8 bits mode)            
-                                           //    N=0 (1 line mode), N=1 (2 line mode)
-                                           //   RE=1 (Ena Extended Regs, special mode for HD66712)
-                                           //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for HD66712)                                
-                                           //   LP=0  (LP=1 Low power mode, LP=0 Normal)                                                                                                                                
-
-          _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
-                                           //   FW=0  (5-dot font, special mode for HD66712)
-                                           //   BW=0  (Cur BW invert disable, special mode for HD66712)
-                                           //   NW=0  (1,2 Line), NW=1 (4 line, special mode for HD66712)                                
-
-          _writeCommand(0x10);             // Scroll/Shift set 0001 HS4 HS3 HS2 HS1 (Ext Regs)
-                                           //   Dotscroll/Display shift enable (Special mode for HD66712)
-
-          _writeCommand(0x80);             // Scroll Quantity set 1 0 HDS5 HDS4 HDS3 HDS2 HDS1 HDS0 (Ext Regs)
-                                           //   Scroll quantity (Special mode for HD66712)
-
-          _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
-                                           //   DL=0  (4 bits bus), DL=1 (8 bits mode)             
-                                           //    N=0  (1 line mode), N=1 (2 line mode)
-                                           //   RE=0  (Dis. Extended Regs, special mode for HD66712)
-                                           //   DH=1  (Disp shift enable/disable, special mode for HD66712)
-                                           //   REV=0 (Reverse/Normal, special mode for HD66712)
-          break; // case HD66712 Controller
-
-           
-        case ST7066_ACM:                                                // ST7066 4/8 bit, I2C on ACM1602 using a PIC        
+                case LCD24x4D:         // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                default:
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                    _function  = 0x0A;    // Function set 001 DL N RE(0) - - (Std Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (2-line mode), N=0 (1-line mode)
+                    //   RE=0  (Dis. Extended Regs, special mode for HD66712)
+
+                    _function_1 = 0x0C;   // Function set 001 DL N RE(1) BE LP (Ext Regs)
+                    //   DL=0  (4 bits bus)
+                    //    N=1  (2 line mode), N=0 (1-line mode)
+                    //   RE=1  (Ena Extended Regs, special mode for HD66712)
+                    //   BE=0  (Blink Enable, CG/SEG RAM, special mode for HD66712)
+                    //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+                    _function_x = 0x00;   // Ext Function set 0000 1 FW BW NW (Ext Regs)
+                    //   NW=0  (1,2 line), NW=1 (4 Line, special mode for HD66712)
+                    break;
+            } // switch type
+
+            // init special features
+            _writeCommand(0x20 | _function_1);// Function set 001 DL N RE(1) BE LP (Ext Regs)
+            //   DL=0 (4 bits bus), DL=1 (8 bits mode)
+            //    N=0 (1 line mode), N=1 (2 line mode)
+            //   RE=1 (Ena Extended Regs, special mode for HD66712)
+            //   BE=0 (Blink Enable/Disable, CG/SEG RAM, special mode for HD66712)
+            //   LP=0  (LP=1 Low power mode, LP=0 Normal)
+
+            _writeCommand(0x08 | _function_x); // Ext Function set 0000 1 FW BW NW (Ext Regs)
+            //   FW=0  (5-dot font, special mode for HD66712)
+            //   BW=0  (Cur BW invert disable, special mode for HD66712)
+            //   NW=0  (1,2 Line), NW=1 (4 line, special mode for HD66712)
+
+            _writeCommand(0x10);             // Scroll/Shift set 0001 HS4 HS3 HS2 HS1 (Ext Regs)
+            //   Dotscroll/Display shift enable (Special mode for HD66712)
+
+            _writeCommand(0x80);             // Scroll Quantity set 1 0 HDS5 HDS4 HDS3 HDS2 HDS1 HDS0 (Ext Regs)
+            //   Scroll quantity (Special mode for HD66712)
+
+            _writeCommand(0x20 | _function); // Function set 001 DL N RE(0) DH REV (Std Regs)
+            //   DL=0  (4 bits bus), DL=1 (8 bits mode)
+            //    N=0  (1 line mode), N=1 (2 line mode)
+            //   RE=0  (Dis. Extended Regs, special mode for HD66712)
+            //   DH=1  (Disp shift enable/disable, special mode for HD66712)
+            //   REV=0 (Reverse/Normal, special mode for HD66712)
+            break; // case HD66712 Controller
+
+
+        case ST7066_ACM:                                                // ST7066 4/8 bit, I2C on ACM1602 using a PIC
         default:
-          // Devices fully compatible to HD44780 that do not use any DC/DC Voltage converters but external VLCD, no icons etc
-
-          // Initialise Display configuration
-          switch (_type) {
-            case LCD8x1:         //8x1 is a regular 1 line display
-            case LCD8x2B:        //8x2B is a special case of 16x1
-//            case LCD12x1:                                
-            case LCD16x1:                                            
-//            case LCD20x1:                                                        
-            case LCD24x1:
-//            case LCD40x1:            
-              _function = 0x00;    // Function set 001 DL N F - -
-                                   //  DL=0 (4 bits bus)             
-                                   //   N=0 (1 line)
-                                   //   F=0 (5x7 dots font)
-              break;                                
-                                                  
-            case LCD12x3D:            // Special mode for KS0078 and PCF21XX                            
-            case LCD12x3D1:           // Special mode for KS0078 and PCF21XX                     
-            case LCD12x4D:            // Special mode for KS0078 and PCF21XX:
-            case LCD16x3D:            // Special mode for KS0078
+            // Devices fully compatible to HD44780 that do not use any DC/DC Voltage converters but external VLCD, no icons etc
+
+            // Initialise Display configuration
+            switch (_type) {
+                case LCD8x1:         //8x1 is a regular 1 line display
+                case LCD8x2B:        //8x2B is a special case of 16x1
+//            case LCD12x1:
+                case LCD16x1:
+//            case LCD20x1:
+                case LCD24x1:
+//            case LCD40x1:
+                    _function = 0x00;    // Function set 001 DL N F - -
+                    //  DL=0 (4 bits bus)
+                    //   N=0 (1 line)
+                    //   F=0 (5x7 dots font)
+                    break;
+
+                case LCD12x3D:            // Special mode for KS0078 and PCF21XX
+                case LCD12x3D1:           // Special mode for KS0078 and PCF21XX
+                case LCD12x4D:            // Special mode for KS0078 and PCF21XX:
+                case LCD16x3D:            // Special mode for KS0078
 //            case LCD16x3D1:           // Special mode for KS0078
 //            case LCD24x3D:            // Special mode for KS0078
-//            case LCD24x3D1:           // Special mode for KS0078            
-            case LCD24x4D:            // Special mode for KS0078
-              error("Error: LCD Controller type does not support this Display type\n\r"); 
-              break;  
-
-            // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
-            default:
-              _function = 0x08;    // Function set 001 DL N F - -
-                                   //  DL=0 (4 bits bus)
-                                   //  Note: 4 bit mode is ignored for native SPI and I2C devices                                 
-                                   //   N=1 (2 lines)
-                                   //   F=0 (5x7 dots font, only option for 2 line display)
-                                   //    -  (Don't care)
-              break;
-          } // switch type
-
-          _writeCommand(0x20 | _function);                         
-          break; // case default Controller
-          
-    } // switch Controller specific initialisations    
-
-    // Controller general initialisations                                          
+//            case LCD24x3D1:           // Special mode for KS0078
+                case LCD24x4D:            // Special mode for KS0078
+                    error("Error: LCD Controller type does not support this Display type\n\r");
+                    break;
+
+                    // All other LCD types are initialised as 2 Line displays (including LCD16x1C and LCD40x4)
+                default:
+                    _function = 0x08;    // Function set 001 DL N F - -
+                    //  DL=0 (4 bits bus)
+                    //  Note: 4 bit mode is ignored for native SPI and I2C devices
+                    //   N=1 (2 lines)
+                    //   F=0 (5x7 dots font, only option for 2 line display)
+                    //    -  (Don't care)
+                    break;
+            } // switch type
+
+            _writeCommand(0x20 | _function);
+            break; // case default Controller
+
+    } // switch Controller specific initialisations
+
+    // Controller general initialisations
 //    _writeCommand(0x01); // Clear Display and set cursor to 0
 //    wait_ms(10);         // The CLS command takes 1.64 ms.
-//                         // Since we are not using the Busy flag, Lets be safe and take 10 ms  
+//                         // Since we are not using the Busy flag, Lets be safe and take 10 ms
 
     _writeCommand(0x02); // Cursor Home, DDRAM Address to Origin
     wait_ms(10);         // The Return Home command takes 1.64 ms.
-                         // Since we are not using the Busy flag, Lets be safe and take 10 ms      
-
-    _writeCommand(0x06); // Entry Mode 0000 0 1 I/D S 
-                         //   Cursor Direction and Display Shift
-                         //   I/D=1 (Cur incr)
-                         //     S=0 (No display shift)                        
-
-    _writeCommand(0x14); // Cursor or Display shift 0001 S/C R/L x x 
-                         //   S/C=0 Cursor moves
-                         //   R/L=1 Right
-                         // 
+    // Since we are not using the Busy flag, Lets be safe and take 10 ms
+
+    _writeCommand(0x06); // Entry Mode 0000 0 1 I/D S
+    //   Cursor Direction and Display Shift
+    //   I/D=1 (Cur incr)
+    //     S=0 (No display shift)
+
+    _writeCommand(0x14); // Cursor or Display shift 0001 S/C R/L x x
+    //   S/C=0 Cursor moves
+    //   R/L=1 Right
+    //
 
 //    _writeCommand(0x0C); // Display Ctrl 0000 1 D C B
-//                         //   Display On, Cursor Off, Blink Off   
-
-//    setCursor(CurOff_BlkOff);     
-    setCursor(CurOn_BlkOff);        
-    setMode(DispOn);     
+//                         //   Display On, Cursor Off, Blink Off
+
+//    setCursor(CurOff_BlkOff);
+    setCursor(CurOn_BlkOff);
+    setMode(DispOn);
 }
 
 
-/** Clear the screen, Cursor home. 
+/** Clear the screen, Cursor home.
   * Note: The whole display is initialised to charcode 0x20, which may not be a 'space' on some controllers with a
   *       different fontset such as the PCF2116C or PCF2119R. In this case you should fill the display with 'spaces'.
   */
-void TextLCD_Base::cls() {
-
-  // Select and configure second LCD controller when needed
-  if(_type==LCD40x4) {
-    _ctrl_idx=_LCDCtrl_1; // Select 2nd controller
-
-    // Second LCD controller Cursor always Off
-    _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
-
-    // Second LCD controller Clearscreen
-    _writeCommand(0x01);  // cls, and set cursor to 0    
-    wait_ms(20);          // The CLS command takes 1.64 ms.
-                          // Since we are not using the Busy flag, Lets be safe and take 10 ms
-  
-    _ctrl_idx=_LCDCtrl_0; // Select primary controller
-  }
-  
-  // Primary LCD controller Clearscreen
-  _writeCommand(0x01);    // cls, and set cursor to 0
-  wait_ms(20);            // The CLS command takes 1.64 ms.
-                          // Since we are not using the Busy flag, Lets be safe and take 10 ms
-
-  // Restore cursormode on primary LCD controller when needed
-  if(_type==LCD40x4) {
-    _setCursorAndDisplayMode(_currentMode,_currentCursor);     
-  }
-                   
-  setAddress(0, 0);  // Reset Cursor location
-                     // Note: This is needed because some displays (eg PCF21XX) don't use line 0 in the '3 Line' mode.   
+void TextLCD_Base::cls()
+{
+
+    // Select and configure second LCD controller when needed
+    if(_type==LCD40x4) {
+        _ctrl_idx=_LCDCtrl_1; // Select 2nd controller
+
+        // Second LCD controller Cursor always Off
+        _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
+
+        // Second LCD controller Clearscreen
+        _writeCommand(0x01);  // cls, and set cursor to 0
+        wait_ms(20);          // The CLS command takes 1.64 ms.
+        // Since we are not using the Busy flag, Lets be safe and take 10 ms
+
+        _ctrl_idx=_LCDCtrl_0; // Select primary controller
+    }
+
+    // Primary LCD controller Clearscreen
+    _writeCommand(0x01);    // cls, and set cursor to 0
+    wait_ms(20);            // The CLS command takes 1.64 ms.
+    // Since we are not using the Busy flag, Lets be safe and take 10 ms
+
+    // Restore cursormode on primary LCD controller when needed
+    if(_type==LCD40x4) {
+        _setCursorAndDisplayMode(_currentMode,_currentCursor);
+    }
+
+    setAddress(0, 0);  // Reset Cursor location
+    // Note: This is needed because some displays (eg PCF21XX) don't use line 0 in the '3 Line' mode.
 }
 
 /** Locate cursor to a screen column and row
   *
   * @param column  The horizontal position from the left, indexed from 0
   * @param row     The vertical position from the top, indexed from 0
-  */ 
-void TextLCD_Base::locate(int column, int row) {
-    
-   // setAddress() does all the heavy lifting:
-   //   check column and row sanity, 
-   //   switch controllers for LCD40x4 if needed
-   //   switch cursor for LCD40x4 if needed
-   //   set the new memory address to show cursor at correct location
-   setAddress(column, row);      
+  */
+void TextLCD_Base::locate(int column, int row)
+{
+
+    // setAddress() does all the heavy lifting:
+    //   check column and row sanity,
+    //   switch controllers for LCD40x4 if needed
+    //   switch cursor for LCD40x4 if needed
+    //   set the new memory address to show cursor at correct location
+    setAddress(column, row);
 }
-   
+
 
 /** Write a single character (Stream implementation)
   */
-int TextLCD_Base::_putc(int value) {
-  int addr;
-    
+int TextLCD_Base::_putc(int value)
+{
+    int addr;
+
     if (value == '\n') {
-      //No character to write
-      
-      //Update Cursor      
-      _column = 0;
-      _row++;
-      if (_row >= rows()) {
-        _row = 0;
-      }      
-    }
-    else {
-      //Character to write
-#if (LCD_DEF_FONT == 1)      
-      _writeData(value);
-#else
-      _writeData(ASCII_2_LCD(value));
-#endif              
-      //Update Cursor
-      _column++;
-      if (_column >= columns()) {
+        //No character to write
+
+        //Update Cursor
         _column = 0;
         _row++;
         if (_row >= rows()) {
-          _row = 0;
+            _row = 0;
         }
-      }          
+    } else {
+        //Character to write
+#if (LCD_DEF_FONT == 1)
+        _writeData(value);
+#elif (LCD_DEF_FONT == 2) //Cyrilic UTF 2 byte font
+        #if(UTF8_SUPP == 1)
+            value=UTF_LCD(value, first_tab_char_cyr, utf_recode_cyr, &utf_recode_rnd_cyr[0][0]);
+            if (value)
+                _writeData(value);
+        #endif
+#else
+        _writeData(ASCII_2_LCD(value));
+#endif
+        //Update Cursor
+        if (value) {
+            _column++;
+            if (_column >= columns()) {
+                _column = 0;
+                _row++;
+                if (_row >= rows()) {
+                    _row = 0;
+                }
+            }
+        }
     } //else
 
     //Set next memoryaddress, make sure cursor blinks at next location
     addr = getAddress(_column, _row);
     _writeCommand(0x80 | addr);
-            
+
     return value;
 }
 
 
 // get a single character (Stream implementation)
-int TextLCD_Base::_getc() {
+int TextLCD_Base::_getc()
+{
     return -1;
 }
 
@@ -1495,13 +1508,14 @@
   * @param c The character to write to the display
   * @return The character code for the specific fonttable of the controller
   */
-int TextLCD_Base::ASCII_2_LCD (int c) {
-    
+int TextLCD_Base::ASCII_2_LCD (int c)
+{
+
 //LCD_C_FT0 is default for HD44780 and compatible series
     if (_font == LCD_C_FT0) return c;
 
-//LCD_C_FT1 for PCF21XXC series    
-//LCD_C_FT2 for PCF21XXR series    
+//LCD_C_FT1 for PCF21XXC series
+//LCD_C_FT2 for PCF21XXR series
 //Used code from Suga koubou library for PCF2119K and PCF2119R
     if (((c >= ' ') && (c <= '?')) || ((c >= 'A') && (c <= 'Z')) || ((c >= 'a') && (c <= 'z'))) {
         c |= 0x80;
@@ -1511,94 +1525,136 @@
     return c;
 
 //LCD_C_FT2 ...
-//@TODO add more, eg cyrillic 
+//@TODO add more, eg cyrillic
 //@TODO add method to switch between fonts for controllers that support this
 }
 
+#if(UTF8_SUPP == 1)
+/** UTF-8 recode
+  * @param c The character to write to the display
+  * @first_tab_char UTF-8 number first symbol in serial table
+  * @serial_recode table for convert symbol to display code
+  * @rnd_recode two dimension table for random symbol convert to display code
+  * @return The character code for the specific fonttable of the controller
+  */
+int TextLCD_Base::UTF_LCD (int c, int first_tab_char, const char *serial_recode, const short int *rnd_recode)
+{
+    int utf_code;
+    char utf_low_byte;
+
+    if (c>=0x80) { // UTF-8 handling
+        if (c >= 0b11000000) { //(0xc0) first UTF-8 byte
+            utf_hi_char = c&0b00011111;
+            return 0;
+        } else if (c<=0b10111111) { //0xbf second UTF-8 byte
+            utf_low_byte= c&0b00111111; //reset 2 hi bit (0x3f)
+            utf_code=(utf_hi_char*0x40+utf_low_byte);
+
+            //Recode leter not in serial table (from random table)
+            for (char i=0; rnd_recode[i*2]; i++) {
+                if (utf_code==rnd_recode[i*2]) {
+                    c=rnd_recode[i*2+1];
+                    return c;
+                }
+            }
+            //Recode from serial table
+            c=serial_recode[utf_code - first_tab_char];
+        }
+    }
+    return c;
+}
+#endif
 
 #if(LCD_PRINTF != 1)
 /** Write a character to the LCD
   *
   * @param c The character to write to the display
   */
-int TextLCD_Base::putc(int c){
-  return _putc(c);
-}  
+int TextLCD_Base::putc(int c)
+{
+    return _putc(c);
+}
 
 
 /** Write a raw string to the LCD
   *
   * @param string text, may be followed by variables to emulate formatting the string.
-  *                     However, printf formatting is NOT supported and variables will be ignored! 
+  *                     However, printf formatting is NOT supported and variables will be ignored!
   */
-int TextLCD_Base::printf(const char* text, ...) {
-  
-  while (*text !=0) {
-    _putc(*text);
-    text++;
-  }
-  return 0;
+int TextLCD_Base::printf(const char* text, ...)
+{
+
+    while (*text !=0) {
+        _putc(*text);
+        text++;
+    }
+    return 0;
 }
-#endif    
+#endif
 
 
 // Write a nibble using the 4-bit interface
-void TextLCD_Base::_writeNibble(int value) {
+void TextLCD_Base::_writeNibble(int value)
+{
 
 // Enable is Low
-    this->_setEnable(true);        
+    this->_setEnable(true);
     this->_setData(value);        // Low nibble of value on D4..D7
-    wait_us(1); // Data setup time        
-    this->_setEnable(false);    
+    wait_us(1); // Data setup time
+    this->_setEnable(false);
     wait_us(1); // Datahold time
 // Enable is Low
 }
 
 // Write a byte using the 4-bit interface
-void TextLCD_Base::_writeByte(int value) {
+void TextLCD_Base::_writeByte(int value)
+{
 
 // Enable is Low
-    this->_setEnable(true);          
+    this->_setEnable(true);
     this->_setData(value >> 4);   // High nibble
-    wait_us(1); // Data setup time    
-    this->_setEnable(false);   
+    wait_us(1); // Data setup time
+    this->_setEnable(false);
     wait_us(1); // Data hold time
-    
-    this->_setEnable(true);        
+
+    this->_setEnable(true);
     this->_setData(value);        // Low nibble
-    wait_us(1); // Data setup time        
-    this->_setEnable(false);    
+    wait_us(1); // Data setup time
+    this->_setEnable(false);
     wait_us(1); // Datahold time
 
 // Enable is Low
 }
 
 // Write a command byte to the LCD controller
-void TextLCD_Base::_writeCommand(int command) {
-
-    this->_setRS(false);        
-    wait_us(1);  // Data setup time for RS       
-    
-    this->_writeByte(command);   
-    wait_us(40); // most instructions take 40us            
+void TextLCD_Base::_writeCommand(int command)
+{
+
+    this->_setRS(false);
+    wait_us(1);  // Data setup time for RS
+
+    this->_writeByte(command);
+    wait_us(40); // most instructions take 40us
 }
 
 // Write a data byte to the LCD controller
-void TextLCD_Base::_writeData(int data) {
-
-    this->_setRS(true);            
-    wait_us(1);  // Data setup time for RS 
-        
+void TextLCD_Base::_writeData(int data)
+{
+
+    this->_setRS(true);
+    wait_us(1);  // Data setup time for RS
+
     this->_writeByte(data);
-    wait_us(40); // data writes take 40us                
+    wait_us(40); // data writes take 40us
 }
 
 
 // This replaces the original _address() method.
 // It is confusing since it returns the memoryaddress or-ed with the set memorycommand 0x80.
 // Left it in here for compatibility with older code. New applications should use getAddress() instead.
-int TextLCD_Base::_address(int column, int row) {
-  return 0x80 | getAddress(column, row);
+int TextLCD_Base::_address(int column, int row)
+{
+    return 0x80 | getAddress(column, row);
 }
 
 
@@ -1611,162 +1667,162 @@
    * @return        The memoryaddress of screen column and row location
    *
    */
-int TextLCD_Base::getAddress(int column, int row) {
+int TextLCD_Base::getAddress(int column, int row)
+{
 
     switch (_addr_mode) {
 
         case LCD_T_A:
-          //Default addressing mode for 1, 2 and 4 rows (except 40x4)
-          //The two available rows are split and stacked on top of eachother. Addressing for 3rd and 4th line continues where lines 1 and 2 were split.          
-          //Displays top rows when less than four are used.          
-          switch (row) {
-            case 0:
-              return 0x00 + column;
-            case 1:
-              return 0x40 + column;
-            case 2:
-              return 0x00 + _nr_cols + column;
-            case 3:
-              return 0x40 + _nr_cols + column;
-            // Should never get here.
-//            default:            
-//              return 0x00;                    
+            //Default addressing mode for 1, 2 and 4 rows (except 40x4)
+            //The two available rows are split and stacked on top of eachother. Addressing for 3rd and 4th line continues where lines 1 and 2 were split.
+            //Displays top rows when less than four are used.
+            switch (row) {
+                case 0:
+                    return 0x00 + column;
+                case 1:
+                    return 0x40 + column;
+                case 2:
+                    return 0x00 + _nr_cols + column;
+                case 3:
+                    return 0x40 + _nr_cols + column;
+                    // Should never get here.
+//            default:
+//              return 0x00;
             }
-          
+
         case LCD_T_B:
-          // LCD8x2B is a special layout of LCD16x1
-          if (row==0) 
-            return 0x00 + column;                        
-          else   
-//            return _nr_cols + column;                                    
-            return 0x08 + column;                        
+            // LCD8x2B is a special layout of LCD16x1
+            if (row==0)
+                return 0x00 + column;
+            else
+//            return _nr_cols + column;
+                return 0x08 + column;
 
         case LCD_T_C:
-          // LCD16x1C is a special layout of LCD8x2
-          // LCD32x1C is a special layout of LCD16x2                    
-          // LCD40x1C is a special layout of LCD20x2          
+            // LCD16x1C is a special layout of LCD8x2
+            // LCD32x1C is a special layout of LCD16x2
+            // LCD40x1C is a special layout of LCD20x2
 #if(0)
-          if (column < 8) 
-            return 0x00 + column;                        
-          else   
-            return 0x40 + (column - 8);                        
+            if (column < 8)
+                return 0x00 + column;
+            else
+                return 0x40 + (column - 8);
 #else
-          if (column < (_nr_cols >> 1)) 
-            return 0x00 + column;                        
-          else   
-            return 0x40 + (column - (_nr_cols >> 1));                        
+            if (column < (_nr_cols >> 1))
+                return 0x00 + column;
+            else
+                return 0x40 + (column - (_nr_cols >> 1));
 #endif
 
 // Not sure about this one, seems wrong.
 // Left in for compatibility with original library
-//        case LCD16x2B:      
+//        case LCD16x2B:
 //            return 0x00 + (row * 40) + column;
 
         case LCD_T_D:
-          //Alternate addressing mode for 3 and 4 row displays (except 40x4). Used by PCF21XX, KS0073, KS0078, SSD1803
-          //The 4 available rows start at a hardcoded address.                    
-          //Displays top rows when less than four are used.
-          switch (row) {
-            case 0:
-              return 0x00 + column;
-            case 1:
-              return 0x20 + column;
-            case 2:
-              return 0x40 + column;
-            case 3:
-              return 0x60 + column;
-            // Should never get here.
-//            default:            
-//              return 0x00;                    
+            //Alternate addressing mode for 3 and 4 row displays (except 40x4). Used by PCF21XX, KS0073, KS0078, SSD1803
+            //The 4 available rows start at a hardcoded address.
+            //Displays top rows when less than four are used.
+            switch (row) {
+                case 0:
+                    return 0x00 + column;
+                case 1:
+                    return 0x20 + column;
+                case 2:
+                    return 0x40 + column;
+                case 3:
+                    return 0x60 + column;
+                    // Should never get here.
+//            default:
+//              return 0x00;
             }
 
         case LCD_T_D1:
-          //Alternate addressing mode for 3 row displays. Used by PCF21XX, KS0073, KS0078, SSD1803
-          //The 4 available rows start at a hardcoded address.                              
-          //Skips top row of 4 row display and starts display at row 1
-          switch (row) {
-            case 0:
-              return 0x20 + column;
-            case 1:
-              return 0x40 + column;
-            case 2:
-              return 0x60 + column;
-            // Should never get here.
-//            default:            
-//              return 0x00;                    
+            //Alternate addressing mode for 3 row displays. Used by PCF21XX, KS0073, KS0078, SSD1803
+            //The 4 available rows start at a hardcoded address.
+            //Skips top row of 4 row display and starts display at row 1
+            switch (row) {
+                case 0:
+                    return 0x20 + column;
+                case 1:
+                    return 0x40 + column;
+                case 2:
+                    return 0x60 + column;
+                    // Should never get here.
+//            default:
+//              return 0x00;
             }
-        
-        case LCD_T_E:                
-          // LCD40x4 is a special case since it has 2 controllers.
-          // Each controller is configured as 40x2 (Type A)
-          if (row<2) { 
-            // Test to see if we need to switch between controllers  
-            if (_ctrl_idx != _LCDCtrl_0) {
-
-              // Second LCD controller Cursor Off
-              _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);    
-
-              // Select primary controller
-              _ctrl_idx = _LCDCtrl_0;
-
-              // Restore cursormode on primary LCD controller
-              _setCursorAndDisplayMode(_currentMode, _currentCursor);    
-            }           
-            
-            return 0x00 + (row * 0x40) + column;          
-          }
-          else {
-
-            // Test to see if we need to switch between controllers  
-            if (_ctrl_idx != _LCDCtrl_1) {
-              // Primary LCD controller Cursor Off
-              _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);    
-
-              // Select secondary controller
-              _ctrl_idx = _LCDCtrl_1;
-
-              // Restore cursormode on secondary LCD controller
-              _setCursorAndDisplayMode(_currentMode, _currentCursor);    
-            }           
-                                   
-            return 0x00 + ((row-2) * 0x40) + column;          
-          } 
-            
+
+        case LCD_T_E:
+            // LCD40x4 is a special case since it has 2 controllers.
+            // Each controller is configured as 40x2 (Type A)
+            if (row<2) {
+                // Test to see if we need to switch between controllers
+                if (_ctrl_idx != _LCDCtrl_0) {
+
+                    // Second LCD controller Cursor Off
+                    _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
+
+                    // Select primary controller
+                    _ctrl_idx = _LCDCtrl_0;
+
+                    // Restore cursormode on primary LCD controller
+                    _setCursorAndDisplayMode(_currentMode, _currentCursor);
+                }
+
+                return 0x00 + (row * 0x40) + column;
+            } else {
+
+                // Test to see if we need to switch between controllers
+                if (_ctrl_idx != _LCDCtrl_1) {
+                    // Primary LCD controller Cursor Off
+                    _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
+
+                    // Select secondary controller
+                    _ctrl_idx = _LCDCtrl_1;
+
+                    // Restore cursormode on secondary LCD controller
+                    _setCursorAndDisplayMode(_currentMode, _currentCursor);
+                }
+
+                return 0x00 + ((row-2) * 0x40) + column;
+            }
+
         case LCD_T_F:
-          //Alternate addressing mode for 3 row displays.
-          //The first half of 3rd row continues from 1st row, the second half continues from 2nd row.                              
-          switch (row) {
-            case 0:
-              return 0x00 + column;
-            case 1:
-              return 0x40 + column;
-            case 2:
-              if (column < (_nr_cols >> 1)) // check first or second half of line
-                return (0x00 + _nr_cols + column);                        
-              else   
-                return (0x40 + _nr_cols + (column - (_nr_cols >> 1)));                        
-            // Should never get here.
-//            default:            
-//              return 0x00;                    
-          }
+            //Alternate addressing mode for 3 row displays.
+            //The first half of 3rd row continues from 1st row, the second half continues from 2nd row.
+            switch (row) {
+                case 0:
+                    return 0x00 + column;
+                case 1:
+                    return 0x40 + column;
+                case 2:
+                    if (column < (_nr_cols >> 1)) // check first or second half of line
+                        return (0x00 + _nr_cols + column);
+                    else
+                        return (0x40 + _nr_cols + (column - (_nr_cols >> 1)));
+                    // Should never get here.
+//            default:
+//              return 0x00;
+            }
 
         case LCD_T_G:
-          //Alternate addressing mode for 3 row displays. Used by ST7036
-          switch (row) {
-            case 0:
-              return 0x00 + column;
-            case 1:
-              return 0x10 + column;
-            case 2:
-              return 0x20 + column;
+            //Alternate addressing mode for 3 row displays. Used by ST7036
+            switch (row) {
+                case 0:
+                    return 0x00 + column;
+                case 1:
+                    return 0x10 + column;
+                case 2:
+                    return 0x20 + column;
+                    // Should never get here.
+//            default:
+//              return 0x00;
+            }
+
             // Should never get here.
-//            default:            
-//              return 0x00;                    
-            }
-
-        // Should never get here.
-        default:            
-            return 0x00;        
+        default:
+            return 0x00;
 
     } // switch _addr_mode
 }
@@ -1777,30 +1833,29 @@
   * @param column  The horizontal position from the left, indexed from 0
   * @param row     The vertical position from the top, indexed from 0
   */
-void TextLCD_Base::setAddress(int column, int row) {
-   
+void TextLCD_Base::setAddress(int column, int row)
+{
+
 // Sanity Check column
     if (column < 0) {
-      _column = 0;
-    }
-    else if (column >= _nr_cols) {
-      _column = _nr_cols - 1;
+        _column = 0;
+    } else if (column >= _nr_cols) {
+        _column = _nr_cols - 1;
     } else _column = column;
-    
+
 // Sanity Check row
     if (row < 0) {
-      _row = 0;
-    }
-    else if (row >= _nr_rows) {
-      _row = _nr_rows - 1;
+        _row = 0;
+    } else if (row >= _nr_rows) {
+        _row = _nr_rows - 1;
     } else _row = row;
-    
-    
+
+
 // Compute the memory address
 // For LCD40x4:  switch controllers if needed
 //               switch cursor if needed
     int addr = getAddress(_column, _row);
-    
+
     _writeCommand(0x80 | addr);
 }
 
@@ -1809,310 +1864,314 @@
   *
   * @return  The number of columns
   *
-  * Note: some configurations are commented out because they have not yet been tested due to lack of hardware     
-  */   
-int TextLCD_Base::columns() {
-    
-  // Columns encoded in b7..b0
-  //return (_type & 0xFF);          
-  return _nr_cols;           
+  * Note: some configurations are commented out because they have not yet been tested due to lack of hardware
+  */
+int TextLCD_Base::columns()
+{
+
+    // Columns encoded in b7..b0
+    //return (_type & 0xFF);
+    return _nr_cols;
 }
 
 /** Return the number of rows
   *
   * @return  The number of rows
   *
-  * Note: some configurations are commented out because they have not yet been tested due to lack of hardware     
+  * Note: some configurations are commented out because they have not yet been tested due to lack of hardware
   */
-int TextLCD_Base::rows() {
-
-  // Rows encoded in b15..b8  
-  //return ((_type >> 8) & 0xFF); 
-  return _nr_rows;          
+int TextLCD_Base::rows()
+{
+
+    // Rows encoded in b15..b8
+    //return ((_type >> 8) & 0xFF);
+    return _nr_rows;
 }
 
 /** Set the Cursormode
   *
   * @param cursorMode  The Cursor mode (CurOff_BlkOff, CurOn_BlkOff, CurOff_BlkOn, CurOn_BlkOn)
   */
-void TextLCD_Base::setCursor(LCDCursor cursorMode) { 
-
-  // Save new cursor mode, needed when 2 controllers are in use or when display is switched off/on
-  _currentCursor = cursorMode;
-    
-  // Configure only current LCD controller
-  _setCursorAndDisplayMode(_currentMode, _currentCursor);    
+void TextLCD_Base::setCursor(LCDCursor cursorMode)
+{
+
+    // Save new cursor mode, needed when 2 controllers are in use or when display is switched off/on
+    _currentCursor = cursorMode;
+
+    // Configure only current LCD controller
+    _setCursorAndDisplayMode(_currentMode, _currentCursor);
 }
 
 /** Set the Displaymode
   *
   * @param displayMode The Display mode (DispOff, DispOn)
   */
-void TextLCD_Base::setMode(LCDMode displayMode) { 
-
-  // Save new displayMode, needed when 2 controllers are in use or when cursor is changed
-  _currentMode = displayMode;
-    
-  // Select and configure second LCD controller when needed
-  if(_type==LCD40x4) {
-    if (_ctrl_idx==_LCDCtrl_0) {      
-      // Configure primary LCD controller
-      _setCursorAndDisplayMode(_currentMode, _currentCursor);
-
-      // Select 2nd controller
-      _ctrl_idx=_LCDCtrl_1;
-  
-      // Configure secondary LCD controller    
-      _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
-
-      // Restore current controller
-      _ctrl_idx=_LCDCtrl_0;       
+void TextLCD_Base::setMode(LCDMode displayMode)
+{
+
+    // Save new displayMode, needed when 2 controllers are in use or when cursor is changed
+    _currentMode = displayMode;
+
+    // Select and configure second LCD controller when needed
+    if(_type==LCD40x4) {
+        if (_ctrl_idx==_LCDCtrl_0) {
+            // Configure primary LCD controller
+            _setCursorAndDisplayMode(_currentMode, _currentCursor);
+
+            // Select 2nd controller
+            _ctrl_idx=_LCDCtrl_1;
+
+            // Configure secondary LCD controller
+            _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
+
+            // Restore current controller
+            _ctrl_idx=_LCDCtrl_0;
+        } else {
+            // Select primary controller
+            _ctrl_idx=_LCDCtrl_0;
+
+            // Configure primary LCD controller
+            _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
+
+            // Restore current controller
+            _ctrl_idx=_LCDCtrl_1;
+
+            // Configure secondary LCD controller
+            _setCursorAndDisplayMode(_currentMode, _currentCursor);
+        }
+    } else {
+        // Configure primary LCD controller
+        _setCursorAndDisplayMode(_currentMode, _currentCursor);
     }
-    else {
-      // Select primary controller
-      _ctrl_idx=_LCDCtrl_0;
-    
-      // Configure primary LCD controller
-      _setCursorAndDisplayMode(_currentMode, CurOff_BlkOff);
-       
-      // Restore current controller
-      _ctrl_idx=_LCDCtrl_1;
-
-      // Configure secondary LCD controller    
-      _setCursorAndDisplayMode(_currentMode, _currentCursor);
-    }
-  }
-  else {
-    // Configure primary LCD controller
-    _setCursorAndDisplayMode(_currentMode, _currentCursor);
-  }       
 }
 
 /** Low level method to restore the cursortype and display mode for current controller
-  */     
-void TextLCD_Base::_setCursorAndDisplayMode(LCDMode displayMode, LCDCursor cursorType) {    
-
-  // Configure current LCD controller   
-  switch (_ctrl) {
-    case ST7070: 
-      //ST7070 does not support Cursorblink. The P bit selects the font instead !   
-      _writeCommand(0x08 | displayMode | (cursorType & 0x02));    
-      break;
-    default:      
-      _writeCommand(0x08 | displayMode | cursorType);
-      break;
-  } //switch      
+  */
+void TextLCD_Base::_setCursorAndDisplayMode(LCDMode displayMode, LCDCursor cursorType)
+{
+
+    // Configure current LCD controller
+    switch (_ctrl) {
+        case ST7070:
+            //ST7070 does not support Cursorblink. The P bit selects the font instead !
+            _writeCommand(0x08 | displayMode | (cursorType & 0x02));
+            break;
+        default:
+            _writeCommand(0x08 | displayMode | cursorType);
+            break;
+    } //switch
 }
 
 /** Set the Backlight mode
   *
   *  @param backlightMode The Backlight mode (LightOff, LightOn)
   */
-void TextLCD_Base::setBacklight(LCDBacklight backlightMode) {
-
-#if (BACKLIGHT_INV==0)      
+void TextLCD_Base::setBacklight(LCDBacklight backlightMode)
+{
+
+#if (BACKLIGHT_INV==0)
     // Positive Backlight control pin logic
     if (backlightMode == LightOn) {
-      this->_setBL(true);     
-    }
-    else {
-      this->_setBL(false);    
+        this->_setBL(true);
+    } else {
+        this->_setBL(false);
     }
 #else
     // Inverted Backlight control pin logic
     if (backlightMode == LightOn) {
-      this->_setBL(false);    
+        this->_setBL(false);
+    } else {
+        this->_setBL(true);
     }
-    else {
-      this->_setBL(true);           
-    }
-#endif    
-} 
+#endif
+}
 
 /** Set User Defined Characters
   *
-  * @param unsigned char c   The Index of the UDC (0..7) for HD44780 or clones and (0..15) for some more advanced controllers 
-  * @param char *udc_data    The bitpatterns for the UDC (8 bytes of 5 significant bits for bitpattern and 3 bits for blinkmode (advanced types))     
+  * @param unsigned char c   The Index of the UDC (0..7) for HD44780 or clones and (0..15) for some more advanced controllers
+  * @param char *udc_data    The bitpatterns for the UDC (8 bytes of 5 significant bits for bitpattern and 3 bits for blinkmode (advanced types))
   */
-void TextLCD_Base::setUDC(unsigned char c, char *udc_data) {
-  
-  // Select and configure second LCD controller when needed
-  if(_type==LCD40x4) {
-    _LCDCtrl_Idx current_ctrl_idx = _ctrl_idx; // Temp save current controller
-   
-    // Select primary controller     
-    _ctrl_idx=_LCDCtrl_0;
-    
-    // Configure primary LCD controller
-    _setUDC(c, udc_data);
-
-    // Select 2nd controller
-    _ctrl_idx=_LCDCtrl_1;
-  
-    // Configure secondary LCD controller    
-    _setUDC(c, udc_data);
-
-    // Restore current controller
-    _ctrl_idx=current_ctrl_idx;       
-  }
-  else {
-    // Configure primary LCD controller
-    _setUDC(c, udc_data); 
-  }   
+void TextLCD_Base::setUDC(unsigned char c, char *udc_data)
+{
+
+    // Select and configure second LCD controller when needed
+    if(_type==LCD40x4) {
+        _LCDCtrl_Idx current_ctrl_idx = _ctrl_idx; // Temp save current controller
+
+        // Select primary controller
+        _ctrl_idx=_LCDCtrl_0;
+
+        // Configure primary LCD controller
+        _setUDC(c, udc_data);
+
+        // Select 2nd controller
+        _ctrl_idx=_LCDCtrl_1;
+
+        // Configure secondary LCD controller
+        _setUDC(c, udc_data);
+
+        // Restore current controller
+        _ctrl_idx=current_ctrl_idx;
+    } else {
+        // Configure primary LCD controller
+        _setUDC(c, udc_data);
+    }
 }
 
 /** Low level method to store user defined characters for current controller
   *
-  * @param unsigned char c   The Index of the UDC (0..7) for HD44780 clones and (0..15) for some more advanced controllers 
-  * @param char *udc_data    The bitpatterns for the UDC (8 bytes of 5 significant bits for bitpattern and 3 bits for blinkmode (advanced types))       
-  */     
-void TextLCD_Base::_setUDC(unsigned char c, char *udc_data) {
-  
-  switch (_ctrl) {
-    case PCF2103_3V3 : // Some UDCs may be used for Icons                  
-    case PCF2113_3V3 : // Some UDCs may be used for Icons                      
-    case PCF2116_3V3 :          
-    case PCF2116_5V  :              
-    case PCF2119_3V3 : // Some UDCs may be used for Icons
-    case PCF2119R_3V3: // Some UDCs may be used for Icons                 
-      c = c & 0x0F; // mask down to valid range
-      break;
-
-    default:     
-      c = c & 0x07; // mask down to valid range    
-      break;  
-  } //switch _ctrl
-
-  // Select DD RAM for current LCD controller
-  // This is needed to correctly set Bit 6 of the addresspointer for controllers that support 16 UDCs
-  _writeCommand(0x80 | ((c << 3) & 0x40)) ;  
-    
-  // Select CG RAM for current LCD controller
-  _writeCommand(0x40 | ((c << 3) & 0x3F)); //Set CG-RAM address, (note that Bit 6 is retained and can not be set by this command !)
-                                           //8 sequential locations needed per UDC
-  // Store UDC pattern 
-  for (int i=0; i<8; i++) {
-    _writeData(*udc_data++);
-  }
-   
-  //Select DD RAM again for current LCD controller and restore the addresspointer
-  int addr = getAddress(_column, _row);
-  _writeCommand(0x80 | addr);  
+  * @param unsigned char c   The Index of the UDC (0..7) for HD44780 clones and (0..15) for some more advanced controllers
+  * @param char *udc_data    The bitpatterns for the UDC (8 bytes of 5 significant bits for bitpattern and 3 bits for blinkmode (advanced types))
+  */
+void TextLCD_Base::_setUDC(unsigned char c, char *udc_data)
+{
+
+    switch (_ctrl) {
+        case PCF2103_3V3 : // Some UDCs may be used for Icons
+        case PCF2113_3V3 : // Some UDCs may be used for Icons
+        case PCF2116_3V3 :
+        case PCF2116_5V  :
+        case PCF2119_3V3 : // Some UDCs may be used for Icons
+        case PCF2119R_3V3: // Some UDCs may be used for Icons
+            c = c & 0x0F; // mask down to valid range
+            break;
+
+        default:
+            c = c & 0x07; // mask down to valid range
+            break;
+    } //switch _ctrl
+
+    // Select DD RAM for current LCD controller
+    // This is needed to correctly set Bit 6 of the addresspointer for controllers that support 16 UDCs
+    _writeCommand(0x80 | ((c << 3) & 0x40)) ;
+
+    // Select CG RAM for current LCD controller
+    _writeCommand(0x40 | ((c << 3) & 0x3F)); //Set CG-RAM address, (note that Bit 6 is retained and can not be set by this command !)
+    //8 sequential locations needed per UDC
+    // Store UDC pattern
+    for (int i=0; i<8; i++) {
+        _writeData(*udc_data++);
+    }
+
+    //Select DD RAM again for current LCD controller and restore the addresspointer
+    int addr = getAddress(_column, _row);
+    _writeCommand(0x80 | addr);
 }
 
 #if(LCD_BLINK == 1)
 /** Set UDC Blink and Icon blink
-  * setUDCBlink method is supported by some compatible devices (eg SSD1803) 
+  * setUDCBlink method is supported by some compatible devices (eg SSD1803)
   *
   * @param blinkMode The Blink mode (BlinkOff, BlinkOn)
   */
-void TextLCD_Base::setUDCBlink(LCDBlink blinkMode){
-  // Blinking UDCs (and icons) are enabled when a specific controlbit (BE) is set.
-  // The blinking pixels in the UDC and icons can be controlled by setting additional bits in the UDC or icon bitpattern.
-  // UDCs are defined by an 8 byte bitpattern. The P0..P4 form the character pattern.
-  //     P7 P6 P5 P4 P3 P2 P1 P0 
-  // 0   B1 B0  x  0  1  1  1  0
-  // 1   B1 B0  x  1  0  0  0  1
-  //        .............
-  // 7   B1 B0  x  1  0  0  0  1
-  //
-  // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE. 
-  //     B1 B0  Mode
-  //      0  0  No Blinking in this row of the UDC
-  //      0  1  Enabled pixels in P4 will blink
-  //      1  x  Enabled pixels in P0..P4 will blink
-  //
-  // Note: the PCF2103 and PCF2113 use UDCs to set Icons 
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6) 
-  // Note: the PCF2119 uses UDCs to set Icons 
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7) 
-  switch (blinkMode) {
-    case BlinkOn: 
-      // Controllers that support UDC/Icon Blink  
-      switch (_ctrl) {
-        case KS0073 :
-        case KS0078 :
-        case HD66712 :                    
-          _function_1 |= 0x02; // Enable UDC/Icon Blink        
-          _writeCommand(0x20 | _function_1);        // Function set 0 0 1 DL N RE(1) BE 0/LP (Ext Regs)
-
-          _writeCommand(0x20 | _function);          // Function set 0 0 1 DL N RE(0) DH REV (Std Regs)
-          break; // case KS0073, KS0078, HD66712 Controller
-    
-        case US2066_3V3 :  
-        case SSD1803_3V3 :  
-          _function_1 |= 0x04; // Enable UDC/Icon Blink
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV 
-                                                    // Select Ext Instr Set
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0
-          break; // case SSD1803, US2066
-
-        case PCF2103_3V3 :  
-        case PCF2113_3V3 :  
-        case PCF2119_3V3 :                  
-        case PCF2119R_3V3 :                          
-          // Enable Icon Blink
-          _writeCommand(0x20 | _function | 0x01);   // Set function, Select Instr Set = 1              
-          _writeCommand(0x08 | 0x02);               // ICON Conf 0000 1, IM=0 (Char mode), IB=1 (Icon blink), 0 (Instr. Set 1) 
-          _writeCommand(0x20 | _function);          // Set function, Select Instr Set = 0             
-
-          break; 
-       
+void TextLCD_Base::setUDCBlink(LCDBlink blinkMode)
+{
+    // Blinking UDCs (and icons) are enabled when a specific controlbit (BE) is set.
+    // The blinking pixels in the UDC and icons can be controlled by setting additional bits in the UDC or icon bitpattern.
+    // UDCs are defined by an 8 byte bitpattern. The P0..P4 form the character pattern.
+    //     P7 P6 P5 P4 P3 P2 P1 P0
+    // 0   B1 B0  x  0  1  1  1  0
+    // 1   B1 B0  x  1  0  0  0  1
+    //        .............
+    // 7   B1 B0  x  1  0  0  0  1
+    //
+    // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE.
+    //     B1 B0  Mode
+    //      0  0  No Blinking in this row of the UDC
+    //      0  1  Enabled pixels in P4 will blink
+    //      1  x  Enabled pixels in P0..P4 will blink
+    //
+    // Note: the PCF2103 and PCF2113 use UDCs to set Icons
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6)
+    // Note: the PCF2119 uses UDCs to set Icons
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7)
+    switch (blinkMode) {
+        case BlinkOn:
+            // Controllers that support UDC/Icon Blink
+            switch (_ctrl) {
+                case KS0073 :
+                case KS0078 :
+                case HD66712 :
+                    _function_1 |= 0x02; // Enable UDC/Icon Blink
+                    _writeCommand(0x20 | _function_1);        // Function set 0 0 1 DL N RE(1) BE 0/LP (Ext Regs)
+
+                    _writeCommand(0x20 | _function);          // Function set 0 0 1 DL N RE(0) DH REV (Std Regs)
+                    break; // case KS0073, KS0078, HD66712 Controller
+
+                case US2066_3V3 :
+                case SSD1803_3V3 :
+                    _function_1 |= 0x04; // Enable UDC/Icon Blink
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV
+                    // Select Ext Instr Set
+
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // case SSD1803, US2066
+
+                case PCF2103_3V3 :
+                case PCF2113_3V3 :
+                case PCF2119_3V3 :
+                case PCF2119R_3V3 :
+                    // Enable Icon Blink
+                    _writeCommand(0x20 | _function | 0x01);   // Set function, Select Instr Set = 1
+                    _writeCommand(0x08 | 0x02);               // ICON Conf 0000 1, IM=0 (Char mode), IB=1 (Icon blink), 0 (Instr. Set 1)
+                    _writeCommand(0x20 | _function);          // Set function, Select Instr Set = 0
+
+                    break;
+
+                default:
+                    //Unsupported feature for other controllers
+                    break;
+            } //switch _ctrl
+
+            break; // BlinkOn
+
+        case BlinkOff:
+            // Controllers that support UDC Blink
+            switch (_ctrl) {
+                case KS0073 :
+                case KS0078 :
+                case HD66712:
+                    _function_1 &= ~0x02; // Disable UDC/Icon Blink
+                    _writeCommand(0x20 | _function_1);        // Function set 0 0 1 DL N RE(1) BE 0/LP (Ext Regs)
+
+                    _writeCommand(0x20 | _function);          // Function set 0 0 1 DL N RE(0) DH REV (Std Regs)
+                    break; // case KS0073, KS0078, HD66712 Controller
+
+                case US2066_3V3 :
+                case SSD1803_3V3 :
+                    _function_1 &= ~0x04; // Disable UDC/Icon Blink
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV
+                    // Select Ext Instr Set
+
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // case SSD1803, US2066
+
+                case PCF2103_3V3 :
+                case PCF2113_3V3 :
+                case PCF2119_3V3 :
+                case PCF2119R_3V3 :
+                    // Disable Icon Blink
+                    _writeCommand(0x20 | _function | 0x01);   // Set function, Select Instr Set = 1
+                    _writeCommand(0x08);                      // ICON Conf 0000 1, IM=0 (Char mode), IB=1 (Icon blink), 0 (Instr. Set 1)
+                    _writeCommand(0x20 | _function);          // Set function, Select Instr Set = 0
+
+                    break;
+
+                default:
+                    //Unsupported feature for other controllers
+                    break;
+            } //switch _ctrl
+
+            break; //BlinkOff
+
         default:
-          //Unsupported feature for other controllers        
-          break; 
-      } //switch _ctrl     
-    
-      break; // BlinkOn 
-
-    case BlinkOff:
-      // Controllers that support UDC Blink  
-      switch (_ctrl) {
-        case KS0073 :
-        case KS0078 :
-        case HD66712:
-          _function_1 &= ~0x02; // Disable UDC/Icon Blink        
-          _writeCommand(0x20 | _function_1);        // Function set 0 0 1 DL N RE(1) BE 0/LP (Ext Regs)
-
-          _writeCommand(0x20 | _function);          // Function set 0 0 1 DL N RE(0) DH REV (Std Regs)
-          break; // case KS0073, KS0078, HD66712 Controller
-    
-        case US2066_3V3 :  
-        case SSD1803_3V3 :  
-          _function_1 &= ~0x04; // Disable UDC/Icon Blink
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV 
-                                                    // Select Ext Instr Set
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0
-          break; // case SSD1803, US2066          
- 
-        case PCF2103_3V3 :
-        case PCF2113_3V3 : 
-        case PCF2119_3V3 :
-        case PCF2119R_3V3 :        
-          // Disable Icon Blink
-          _writeCommand(0x20 | _function | 0x01);   // Set function, Select Instr Set = 1              
-          _writeCommand(0x08);                      // ICON Conf 0000 1, IM=0 (Char mode), IB=1 (Icon blink), 0 (Instr. Set 1) 
-          _writeCommand(0x20 | _function);          // Set function, Select Instr Set = 0             
-
-          break; 
-       
-        default:
-          //Unsupported feature for other controllers        
-          break; 
-      } //switch _ctrl     
-    
-      break; //BlinkOff
-      
-    default:
-      break;      
-  } // blinkMode
-  
+            break;
+    } // blinkMode
+
 } // setUDCBlink()
 #endif
 
@@ -2120,147 +2179,148 @@
   * setContrast method is supported by some compatible devices (eg ST7032i) that have onboard LCD voltage generation
   * Initial code for ST70XX imported from fork by JH1PJL
   *
-  * @param unsigned char c   contrast data (6 significant bits, valid range 0..63, Value 0 will disable the Vgen)  
+  * @param unsigned char c   contrast data (6 significant bits, valid range 0..63, Value 0 will disable the Vgen)
   * @return none
   */
 //@TODO Add support for 40x4 dual controller
-void TextLCD_Base::setContrast(unsigned char c) {
+void TextLCD_Base::setContrast(unsigned char c)
+{
 
 // Function set mode stored during Init. Make sure we dont accidentally switch between 1-line and 2-line mode!
 // Icon/Booster mode stored during Init. Make sure we dont accidentally change this!
- 
-  _contrast = c & 0x3F; // Sanity check
-  
-  switch (_ctrl) {   
-    case PCF2113_3V3 :  
-    case PCF2119_3V3 :
-    case PCF2119R_3V3 :    
-       if (_contrast <  5) _contrast = 0;  // See datasheet. Sanity check for PCF2113/PCF2119
-       if (_contrast > 55) _contrast = 55;
-      
-       _writeCommand(0x20 | _function | 0x01);               // Set function, Select Instruction Set = 1              
-       _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=0, VA=contrast
-       _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=1, VB=contrast
-       _writeCommand(0x20 | _function);                      // Select Instruction Set = 0
-       break;
-        
-    case ST7032_3V3 :  
-    case ST7032_5V :      
-    case ST7036_3V3 :      
-//    case ST7036_5V :          
-    case SSD1803_3V3 :      
-      _writeCommand(0x20 | _function | 0x01);                        // Select Instruction Set = 1
-      _writeCommand(0x70 | (_contrast & 0x0F));                      // Contrast Low bits
-      _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03)); // Contrast High bits 
-      _writeCommand(0x20 | _function);                               // Select Instruction Set = 0
-      break;
-
-    case US2066_3V3 :      
-      _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV 
-                                                // Select Extended Instruction Set
-
-      _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
-         
-      _writeCommand(0x81);                      // Set Contrast Control: 1 0 0 0 0 0 0 1 (Ext Instr Set, OLED)
-      _writeCommand((_contrast << 2) | 0x03);   // Set Contrast Value: 8 bits. Use 6 bits for compatibility    
-      
-      _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)          
-
-      _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                // Select Std Instr set, Select IS=0
-      break;
-
-    //not yet tested on hardware
-    case PT6314 :
-      // Only 2 significant bits
-      //   0x00 = 100%
-      //   0x01 =  75%
-      //   0x02 =  50%
-      //   0x03 =  25%                
-      _writeCommand(0x20 | _function | ((~_contrast) >> 4));        // Invert and shift to use 2 MSBs     
-      break;
-            
-    default:  
-      //Unsupported feature for other controllers
-      break;               
-  } // end switch     
+
+    _contrast = c & 0x3F; // Sanity check
+
+    switch (_ctrl) {
+        case PCF2113_3V3 :
+        case PCF2119_3V3 :
+        case PCF2119R_3V3 :
+            if (_contrast <  5) _contrast = 0;  // See datasheet. Sanity check for PCF2113/PCF2119
+            if (_contrast > 55) _contrast = 55;
+
+            _writeCommand(0x20 | _function | 0x01);               // Set function, Select Instruction Set = 1
+            _writeCommand(0x80 | 0x00 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=0, VA=contrast
+            _writeCommand(0x80 | 0x40 | (_contrast & 0x3F));      // VLCD_set (Instr. Set 1)    V=1, VB=contrast
+            _writeCommand(0x20 | _function);                      // Select Instruction Set = 0
+            break;
+
+        case ST7032_3V3 :
+        case ST7032_5V :
+        case ST7036_3V3 :
+//    case ST7036_5V :
+        case SSD1803_3V3 :
+            _writeCommand(0x20 | _function | 0x01);                        // Select Instruction Set = 1
+            _writeCommand(0x70 | (_contrast & 0x0F));                      // Contrast Low bits
+            _writeCommand(0x50 | _icon_power | ((_contrast >> 4) & 0x03)); // Contrast High bits
+            _writeCommand(0x20 | _function);                               // Select Instruction Set = 0
+            break;
+
+        case US2066_3V3 :
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N BE RE(1) REV
+            // Select Extended Instruction Set
+
+            _writeCommand(0x79);                      // Function Select OLED:  0 1 1 1 1 0 0 1 (Ext Instr Set)
+
+            _writeCommand(0x81);                      // Set Contrast Control: 1 0 0 0 0 0 0 1 (Ext Instr Set, OLED)
+            _writeCommand((_contrast << 2) | 0x03);   // Set Contrast Value: 8 bits. Use 6 bits for compatibility
+
+            _writeCommand(0x78);                      // Function Disable OLED: 0 1 1 1 1 0 0 0 (Ext Instr Set)
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+            break;
+
+            //not yet tested on hardware
+        case PT6314 :
+            // Only 2 significant bits
+            //   0x00 = 100%
+            //   0x01 =  75%
+            //   0x02 =  50%
+            //   0x03 =  25%
+            _writeCommand(0x20 | _function | ((~_contrast) >> 4));        // Invert and shift to use 2 MSBs
+            break;
+
+        default:
+            //Unsupported feature for other controllers
+            break;
+    } // end switch
 } // end setContrast()
 
 #if(LCD_POWER == 1)
 /** Set Power
   * setPower method is supported by some compatible devices (eg SSD1803) that have power down modes
   *
-  * @param bool powerOn  Power on/off   
+  * @param bool powerOn  Power on/off
   * @return none
   */
-//@TODO Add support for 40x4 dual controller  
-void TextLCD_Base::setPower(bool powerOn) {
-  
-  if (powerOn) {
-    // Switch on  
-    setMode(DispOn);       
-
-    // Controllers that supports specific Power Down mode
-    switch (_ctrl) {
-    
-//    case PCF2113_3V3 :  
-//    case PCF2119_3V3 : 
-//    case PCF2119R_3V3 :     
-//    case ST7032_3V3 :  
+//@TODO Add support for 40x4 dual controller
+void TextLCD_Base::setPower(bool powerOn)
+{
+
+    if (powerOn) {
+        // Switch on
+        setMode(DispOn);
+
+        // Controllers that supports specific Power Down mode
+        switch (_ctrl) {
+
+//    case PCF2113_3V3 :
+//    case PCF2119_3V3 :
+//    case PCF2119R_3V3 :
+//    case ST7032_3V3 :
 //@todo
 //    enable Booster Bon
 
-      case WS0010:      
-        _writeCommand(0x17);   // Char mode, DC/DC on        
-        wait_ms(10);           // Wait 10ms to ensure powered up             
-        break;
-
-      case KS0073:        
-      case KS0078:        
-      case SSD1803_3V3 :      
-//      case SSD1803_5V :            
-        _writeCommand(0x20 | _function_1);                             // Select Ext Instr Set
-        _writeCommand(0x02);                                           // Power On
-        _writeCommand(0x20 | _function);                               // Select Std Instr Set
-        break;
-                    
-      default:  
-        //Unsupported feature for other controllers
-        break;              
-    } // end switch  
-  }  
-  else {
-    // Switch off        
-    setMode(DispOff);       
-
-    // Controllers that support specific Power Down mode
-    switch (_ctrl) {
-    
-//    case PCF2113_3V3 :  
-//    case PCF2119_3V3 : 
-//    case PCF2119R_3V3 :     
-//    case ST7032_3V3 :  
+            case WS0010:
+                _writeCommand(0x17);   // Char mode, DC/DC on
+                wait_ms(10);           // Wait 10ms to ensure powered up
+                break;
+
+            case KS0073:
+            case KS0078:
+            case SSD1803_3V3 :
+//      case SSD1803_5V :
+                _writeCommand(0x20 | _function_1);                             // Select Ext Instr Set
+                _writeCommand(0x02);                                           // Power On
+                _writeCommand(0x20 | _function);                               // Select Std Instr Set
+                break;
+
+            default:
+                //Unsupported feature for other controllers
+                break;
+        } // end switch
+    } else {
+        // Switch off
+        setMode(DispOff);
+
+        // Controllers that support specific Power Down mode
+        switch (_ctrl) {
+
+//    case PCF2113_3V3 :
+//    case PCF2119_3V3 :
+//    case PCF2119R_3V3 :
+//    case ST7032_3V3 :
 //@todo
 //    disable Booster Bon
 
-      case WS0010:      
-        _writeCommand(0x13);   // Char mode, DC/DC off              
-        break;
-        
-      case KS0073:
-      case KS0078:
-      case SSD1803_3V3 :      
-//      case SSD1803_5V :            
-        _writeCommand(0x20 | _function_1);                             // Select Ext Instr Set
-        _writeCommand(0x03);                                           // Power Down
-        _writeCommand(0x20 | _function);                               // Select Std Instr Set
-        break;
-
-      default:  
-        //Unsupported feature for other controllers
-        break;              
-    } // end switch  
-  }
+            case WS0010:
+                _writeCommand(0x13);   // Char mode, DC/DC off
+                break;
+
+            case KS0073:
+            case KS0078:
+            case SSD1803_3V3 :
+//      case SSD1803_5V :
+                _writeCommand(0x20 | _function_1);                             // Select Ext Instr Set
+                _writeCommand(0x03);                                           // Power Down
+                _writeCommand(0x20 | _function);                               // Select Std Instr Set
+                break;
+
+            default:
+                //Unsupported feature for other controllers
+                break;
+        } // end switch
+    }
 } // end setPower()
 #endif
 
@@ -2268,232 +2328,233 @@
 /** Set Orient
   * setOrient method is supported by some compatible devices (eg SSD1803, US2066) that have top/bottom view modes
   *
-  * @param LCDOrient orient Orientation 
+  * @param LCDOrient orient Orientation
   * @return none
   */
-void TextLCD_Base::setOrient(LCDOrient orient){
-
-  switch (orient) {
-       
-    case Top:
-      switch (_ctrl) {
-        case PCF2103_3V3:              
-        case PCF2116_3V3:        
-        case PCF2116_5V:                
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-          _writeCommand(0x05);                             // Display Conf Set         0000 0, 1, P=0, Q=1               (Instr. Set 1)
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-          break;
-
-        case PCF2119_3V3:   
-        case PCF2119R_3V3:                         
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-          _writeCommand(0x07);                             // Display Conf Set         0000 0, 1, P=1, Q=1               (Instr. Set 1)
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-          break;
-                               
-        case SSD1803_3V3 :      
+void TextLCD_Base::setOrient(LCDOrient orient)
+{
+
+    switch (orient) {
+
+        case Top:
+            switch (_ctrl) {
+                case PCF2103_3V3:
+                case PCF2116_3V3:
+                case PCF2116_5V:
+                    _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+                    _writeCommand(0x05);                             // Display Conf Set         0000 0, 1, P=0, Q=1               (Instr. Set 1)
+                    _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+                    break;
+
+                case PCF2119_3V3:
+                case PCF2119R_3V3:
+                    _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+                    _writeCommand(0x07);                             // Display Conf Set         0000 0, 1, P=1, Q=1               (Instr. Set 1)
+                    _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+                    break;
+
+                case SSD1803_3V3 :
 //      case SSD1803_5V :
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
 //          _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
-          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)          
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0       
-          break;
-
-        case ST7070:      
-          _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL, N, EXT=1, x, x (Select Instr Set = 1)
-
-          _writeCommand(0x40 | 0x00);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
-                                                    // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
-                                                    // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41                                                    
-          wait_ms(5);                               // Wait to ensure completion or ST7070 fails to set Top/Bottom after reset..
-          
-          _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
-        
-          break; // case ST7070 Controller
-          
-        default:  
-          //Unsupported feature for other controllers
-          break;              
-
-      } // end switch _ctrl     
-      break; // end Top
-                
-    case Bottom:
-      switch (_ctrl) {
-        case PCF2103_3V3:              
-        case PCF2116_3V3:        
-        case PCF2116_5V:                
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-          _writeCommand(0x06);                             // Display Conf Set         0000 0, 1, P=1, Q=0               (Instr. Set 1)
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-          break;
-
-        case PCF2119_3V3:
-        case PCF2119R_3V3 :                                 
-          _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1              
-          _writeCommand(0x04);                             // Display Conf Set         0000 0, 1, P=0, Q=0               (Instr. Set 1)
-          _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0             
-          break;
-        
-        case SSD1803_3V3 :      
+                    _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)
+
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break;
+
+                case ST7070:
+                    _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL, N, EXT=1, x, x (Select Instr Set = 1)
+
+                    _writeCommand(0x40 | 0x00);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
+                    // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
+                    // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41
+                    wait_ms(5);                               // Wait to ensure completion or ST7070 fails to set Top/Bottom after reset..
+
+                    _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
+
+                    break; // case ST7070 Controller
+
+                default:
+                    //Unsupported feature for other controllers
+                    break;
+
+            } // end switch _ctrl
+            break; // end Top
+
+        case Bottom:
+            switch (_ctrl) {
+                case PCF2103_3V3:
+                case PCF2116_3V3:
+                case PCF2116_5V:
+                    _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+                    _writeCommand(0x06);                             // Display Conf Set         0000 0, 1, P=1, Q=0               (Instr. Set 1)
+                    _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+                    break;
+
+                case PCF2119_3V3:
+                case PCF2119R_3V3 :
+                    _writeCommand(0x20 | _function | 0x01);          // Set function, Select Instr Set = 1
+                    _writeCommand(0x04);                             // Display Conf Set         0000 0, 1, P=0, Q=0               (Instr. Set 1)
+                    _writeCommand(0x20 | _function);                 // Set function, Select Instr Set = 0
+                    break;
+
+                case SSD1803_3V3 :
 //      case SSD1803_5V :
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
-//          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)          
-
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0       
-          break;
-
-        case ST7070:
-          //Note: this does not result in correct top/bottom view. 
-          //The left and right half of each row are reversed and the addressing of both rows is also incorrect:
-          //Top/bottomline when orientation is flipped:
-          //  0x48...0x4F  0x40...0x47
-          //  0x08...0x0F  0x00...0x07
-          _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1)
-
-          _writeCommand(0x40 | 0x0F);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
-                                                    // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
-                                                    // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41                                                    
-          wait_ms(5);                               // Wait to ensure completion or ST7070 fails to set Top/Bottom after reset..
-          
-          _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
-        
-          break; // case ST7070 Controller
-          
-        default:  
-          //Unsupported feature for other controllers
-          break;              
-
-      } // end switch _ctrl     
-    
-      break; // end Bottom
-  } // end switch orient
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    _writeCommand(0x06);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=1 COM1-32, BDS=0 SEG100-1    "Bottom View" (Ext Instr Set)
+//          _writeCommand(0x05);                      // Set ext entry mode, 0 0 0 0 0 1 BDC=0 COM32-1, BDS=1 SEG1-100    "Top View" (Ext Instr Set)
+
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break;
+
+                case ST7070:
+                    //Note: this does not result in correct top/bottom view.
+                    //The left and right half of each row are reversed and the addressing of both rows is also incorrect:
+                    //Top/bottomline when orientation is flipped:
+                    //  0x48...0x4F  0x40...0x47
+                    //  0x08...0x0F  0x00...0x07
+                    _writeCommand(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1)
+
+                    _writeCommand(0x40 | 0x0F);               // COM/SEG directions 0 1 0 0 C1, C2, S1, S2  (Instr Set 1)
+                    // C1=1: Com1-8 -> Com8-1;   C2=1: Com9-16 -> Com16-9
+                    // S1=1: Seg1-40 -> Seg40-1; S2=1: Seg41-80 -> Seg80-41
+                    wait_ms(5);                               // Wait to ensure completion or ST7070 fails to set Top/Bottom after reset..
+
+                    _writeCommand(0x20 | _function);          // Set function, EXT=0 (Select Instr Set = 0)
+
+                    break; // case ST7070 Controller
+
+                default:
+                    //Unsupported feature for other controllers
+                    break;
+
+            } // end switch _ctrl
+
+            break; // end Bottom
+    } // end switch orient
 } // end setOrient()
 #endif
 
 #if(LCD_BIGFONT == 1)
 /** Set Big Font
-  * setBigFont method is supported by some compatible devices (eg SSD1803, US2066) 
+  * setBigFont method is supported by some compatible devices (eg SSD1803, US2066)
   *
   * @param lines  The selected Big Font lines (None, TopLine, CenterLine, BottomLine, TopBottomLine)
   *                                            Double height characters can be shown on lines 1+2, 2+3, 3+4 or 1+2 and 3+4
   *                                            Valid double height lines depend on the LCDs number of rows.
   */
-void TextLCD_Base::setBigFont(LCDBigFont lines) {
-
-  switch (lines) {
-    case None:
-      switch (_ctrl) {
-        case SSD1803_3V3 :      
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          _writeCommand(0x1C);                      // Double Height, 0 0 0 1 UD2=1, UD1=1, X, DH'=0 (Ext Instr Set)
-                                                    // Default
-          _function = _function & ~0x04;            // Set function, 0 0 1 DL N DH=0 RE(0) IS=0 Select Instruction Set 0
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // end US2066      
-
-        default:
-          break; // end default      
-      } // end switch _ctrl     
-      break; // end None      
-    
-    case TopLine:
-      if (_nr_rows < 2) return; //Sanity check  
-
-      switch (_ctrl) {
-        case SSD1803_3V3 :              
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          _writeCommand(0x1C);                      // Double Height, 0 0 0 1 UD2=1, UD1=1, X, DH'=0 (Ext Instr Set)
-                                                    // Default
-          _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // end US2066, SSD1803      
-
-        default:
-          break; // end default      
-      } // end switch _ctrl     
-      break; // end TopLine              
-
-    case CenterLine:
-      if (_nr_rows != 4) return; //Sanity check  
-          
-      switch (_ctrl) {
-        case SSD1803_3V3 :              
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          _writeCommand(0x14);                      // Double Height, 0 0 0 1 UD2=0, UD1=1, X, DH'=0 (Ext Instr Set)
-                                                    // Default
-          _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // end US2066, SSD1803      
-
-        default:
-          break; // end default      
-      } // end switch _ctrl        
-      break; // end CenterLine              
-
-    case BottomLine:
-      if (_nr_rows < 3) return; //Sanity check  
-          
-      switch (_ctrl) {
-        case SSD1803_3V3 :              
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          if (_nr_rows == 3) {
-            _writeCommand(0x14);                      // Double Height, 0 0 0 1 UD2=0, UD1=1, X, DH'=0 (Ext Instr Set)
-          }  
-          else {
-            _writeCommand(0x10);                      // Double Height, 0 0 0 1 UD2=0, UD1=0, X, DH'=0 (Ext Instr Set)
-          }  
-          _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // end US2066, SSD1803      
-
-        default:
-          break; // end default      
-      } // end switch _ctrl           
-      break; // end BottomLine          
-      
-    case TopBottomLine:
-      if (_nr_rows != 4) return; //Sanity check  
-      
-      switch (_ctrl) {
-        case SSD1803_3V3 :        
-        case US2066_3V3 :      
-          _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV 
-                                                    // Select Extended Instruction Set
-          _writeCommand(0x18);                      // Double Height, 0 0 0 1 UD2=1, UD1=0, X, DH'=0 (Ext Instr Set)
-                                                    // Default
-          _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
-          _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
-                                                    // Select Std Instr set, Select IS=0        
-          break; // end US2066, SSD1803      
-
-        default:
-          break; // end default      
-      } // end switch _ctrl           
-      break; // end TopBottomLine          
-
-  } // end switch lines
+void TextLCD_Base::setBigFont(LCDBigFont lines)
+{
+
+    switch (lines) {
+        case None:
+            switch (_ctrl) {
+                case SSD1803_3V3 :
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    _writeCommand(0x1C);                      // Double Height, 0 0 0 1 UD2=1, UD1=1, X, DH'=0 (Ext Instr Set)
+                    // Default
+                    _function = _function & ~0x04;            // Set function, 0 0 1 DL N DH=0 RE(0) IS=0 Select Instruction Set 0
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // end US2066
+
+                default:
+                    break; // end default
+            } // end switch _ctrl
+            break; // end None
+
+        case TopLine:
+            if (_nr_rows < 2) return; //Sanity check
+
+            switch (_ctrl) {
+                case SSD1803_3V3 :
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    _writeCommand(0x1C);                      // Double Height, 0 0 0 1 UD2=1, UD1=1, X, DH'=0 (Ext Instr Set)
+                    // Default
+                    _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // end US2066, SSD1803
+
+                default:
+                    break; // end default
+            } // end switch _ctrl
+            break; // end TopLine
+
+        case CenterLine:
+            if (_nr_rows != 4) return; //Sanity check
+
+            switch (_ctrl) {
+                case SSD1803_3V3 :
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    _writeCommand(0x14);                      // Double Height, 0 0 0 1 UD2=0, UD1=1, X, DH'=0 (Ext Instr Set)
+                    // Default
+                    _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // end US2066, SSD1803
+
+                default:
+                    break; // end default
+            } // end switch _ctrl
+            break; // end CenterLine
+
+        case BottomLine:
+            if (_nr_rows < 3) return; //Sanity check
+
+            switch (_ctrl) {
+                case SSD1803_3V3 :
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    if (_nr_rows == 3) {
+                        _writeCommand(0x14);                      // Double Height, 0 0 0 1 UD2=0, UD1=1, X, DH'=0 (Ext Instr Set)
+                    } else {
+                        _writeCommand(0x10);                      // Double Height, 0 0 0 1 UD2=0, UD1=0, X, DH'=0 (Ext Instr Set)
+                    }
+                    _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // end US2066, SSD1803
+
+                default:
+                    break; // end default
+            } // end switch _ctrl
+            break; // end BottomLine
+
+        case TopBottomLine:
+            if (_nr_rows != 4) return; //Sanity check
+
+            switch (_ctrl) {
+                case SSD1803_3V3 :
+                case US2066_3V3 :
+                    _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 X N BE RE(1) REV
+                    // Select Extended Instruction Set
+                    _writeCommand(0x18);                      // Double Height, 0 0 0 1 UD2=1, UD1=0, X, DH'=0 (Ext Instr Set)
+                    // Default
+                    _function = _function | 0x04;             // Set function, 0 0 1 DL N DH=1 RE(0) IS=0 Select Instruction Set 0
+                    _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS=0 Select Instruction Set 0
+                    // Select Std Instr set, Select IS=0
+                    break; // end US2066, SSD1803
+
+                default:
+                    break; // end default
+            } // end switch _ctrl
+            break; // end TopBottomLine
+
+    } // end switch lines
 
 } // end setBigFont()
 #endif
@@ -2502,101 +2563,102 @@
 /** Set Icons
   *
   * @param unsigned char idx   The Index of the icon pattern (0..15) for KS0073 and similar controllers
-  *                            and Index (0..31) for PCF2103 and similar controllers  
-  * @param unsigned char data  The bitpattern for the icons (6 lsb for KS0073 bitpattern (5 lsb for KS0078) and 2 msb for blinkmode)       
-  *                            The bitpattern for the PCF2103 icons is 5 lsb (UDC 0..2) and 5 lsb for blinkmode (UDC 4..6)         
+  *                            and Index (0..31) for PCF2103 and similar controllers
+  * @param unsigned char data  The bitpattern for the icons (6 lsb for KS0073 bitpattern (5 lsb for KS0078) and 2 msb for blinkmode)
+  *                            The bitpattern for the PCF2103 icons is 5 lsb (UDC 0..2) and 5 lsb for blinkmode (UDC 4..6)
   */
-void TextLCD_Base::setIcon(unsigned char idx, unsigned char data) {
-  // Blinking icons are enabled when a specific controlbit (BE) is set.
-  // The blinking pixels in the icons can be controlled by setting additional bits in the icon bitpattern.
-  // Icons are defined by a byte bitpattern. The P0..P5 form the Icon pattern for KS0073, and P0..P4 for KS0078
-  //     P7 P6 P5 P4 P3 P2 P1 P0 
-  // 0   B1 B0  0  0  1  1  1  0
-  // 1   B1 B0  1  1  0  0  0  1
-  //        .............
-  // 15  B1 B0  1  1  0  0  0  1
-  //
-  // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE. 
-  //     B1 B0  Mode
-  //      0  0  No Blinking for this icon row
-  //      0  1  Enabled pixels in P5 will blink
-  //      1  x  Enabled pixels in P0..P5 will blink
-  //
-  // Note: the PCF2103 and PCF2113 use UDCs to set Icons 
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6) 
-  // Note: the PCF2119 uses UDCs to set Icons 
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7) 
-  
-  switch (_ctrl) {
-    case KS0073:
-    case KS0078:    
-       _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N RE(1) BE LP 
-                                                 // Select Extended Instruction Set
-       _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Ext Instr Set)
-
-       _writeData(data);                         // Set Icon pattern (Ext Instr Set)
-
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
-                                                 // Select Std Instr set, Select IS=0        
-       break; // end KS0073, KS0078
-
-    case ST7032_3V3:
-    case ST7032_5V:
-       _writeCommand(0x20 | _function | 0x01);   // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1              
-       _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set 1)
-
-       _writeData(data & 0x1F);                  // Set Icon pattern, no blink support (Instr Set 1) 
-
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
-                                                 // Select Std Instr set, Select IS=0        
-       break; // end ST7032
-
-    case ST7036_3V3:
-    case ST7036_5V:
-       _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH IS2,IS1 = 01 (Select Instr Set = 1)    
-       _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set 1)
-
-       _writeData(data & 0x1F);                  // Set Icon pattern, no blink support (Instr Set 1) 
-
-       _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instr Set = 0)    
-                                                 // Select Std Instr set, Select IS=0        
-       break; // end ST7036
-
-    case SSD1803_3V3:                  
-//    case SSD1803_5V:                      
-       _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS 
-                                                 // Select Instruction Set 1
-       _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set = 1)
-       _writeData(data);                         // Set Icon pattern (Instr Set = 1)
-
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS
-                                                 // Select IS=0        
-       break; // end SSD1803      
-
-    case PCF2103_3V3:
-    case PCF2113_3V3:    
-    case PCF2119_3V3:
-    case PCF2119R_3V3:                
-       // Store UDC/Icon pattern for PCF2103 and PCF2113: 
-       //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
-       //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6) 
-       // Store UDC/Icon pattern for PCF2119: 
-       //   4 x 8 rows x 5 bits = 160 bits for Normal pattern (UDC 0..3) and
-       //   4 x 8 rows x 5 bits = 160 bits for Blink pattern (UDC 4..7)        
-       _writeCommand(0x40 | (idx & 0x3F));       //Set CG-RAM address, 8 sequential locations needed per UDC
-       _writeData(data);                         // Set Icon pattern (Instr Set = 1)
-       break; // case PCF2103_3V3 Controller
-                 
-    default:
-       break; // end default      
-  } // end switch _ctrl           
-  
-  //Select DD RAM again for current LCD controller and restore the addresspointer
-  int addr = getAddress(_column, _row);
-  _writeCommand(0x80 | addr);  
-         
+void TextLCD_Base::setIcon(unsigned char idx, unsigned char data)
+{
+    // Blinking icons are enabled when a specific controlbit (BE) is set.
+    // The blinking pixels in the icons can be controlled by setting additional bits in the icon bitpattern.
+    // Icons are defined by a byte bitpattern. The P0..P5 form the Icon pattern for KS0073, and P0..P4 for KS0078
+    //     P7 P6 P5 P4 P3 P2 P1 P0
+    // 0   B1 B0  0  0  1  1  1  0
+    // 1   B1 B0  1  1  0  0  0  1
+    //        .............
+    // 15  B1 B0  1  1  0  0  0  1
+    //
+    // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE.
+    //     B1 B0  Mode
+    //      0  0  No Blinking for this icon row
+    //      0  1  Enabled pixels in P5 will blink
+    //      1  x  Enabled pixels in P0..P5 will blink
+    //
+    // Note: the PCF2103 and PCF2113 use UDCs to set Icons
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6)
+    // Note: the PCF2119 uses UDCs to set Icons
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7)
+
+    switch (_ctrl) {
+        case KS0073:
+        case KS0078:
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N RE(1) BE LP
+            // Select Extended Instruction Set
+            _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Ext Instr Set)
+
+            _writeData(data);                         // Set Icon pattern (Ext Instr Set)
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+            break; // end KS0073, KS0078
+
+        case ST7032_3V3:
+        case ST7032_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1
+            _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set 1)
+
+            _writeData(data & 0x1F);                  // Set Icon pattern, no blink support (Instr Set 1)
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+            break; // end ST7032
+
+        case ST7036_3V3:
+        case ST7036_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH IS2,IS1 = 01 (Select Instr Set = 1)
+            _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set 1)
+
+            _writeData(data & 0x1F);                  // Set Icon pattern, no blink support (Instr Set 1)
+
+            _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instr Set = 0)
+            // Select Std Instr set, Select IS=0
+            break; // end ST7036
+
+        case SSD1803_3V3:
+//    case SSD1803_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS
+            // Select Instruction Set 1
+            _writeCommand(0x40 | (idx & 0x0F));       // Set Icon Address, mask Address to valid range (Instr Set = 1)
+            _writeData(data);                         // Set Icon pattern (Instr Set = 1)
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS
+            // Select IS=0
+            break; // end SSD1803
+
+        case PCF2103_3V3:
+        case PCF2113_3V3:
+        case PCF2119_3V3:
+        case PCF2119R_3V3:
+            // Store UDC/Icon pattern for PCF2103 and PCF2113:
+            //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
+            //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6)
+            // Store UDC/Icon pattern for PCF2119:
+            //   4 x 8 rows x 5 bits = 160 bits for Normal pattern (UDC 0..3) and
+            //   4 x 8 rows x 5 bits = 160 bits for Blink pattern (UDC 4..7)
+            _writeCommand(0x40 | (idx & 0x3F));       //Set CG-RAM address, 8 sequential locations needed per UDC
+            _writeData(data);                         // Set Icon pattern (Instr Set = 1)
+            break; // case PCF2103_3V3 Controller
+
+        default:
+            break; // end default
+    } // end switch _ctrl
+
+    //Select DD RAM again for current LCD controller and restore the addresspointer
+    int addr = getAddress(_column, _row);
+    _writeCommand(0x80 | addr);
+
 } // end setIcon()
 
 /** Clear Icons
@@ -2604,189 +2666,190 @@
   * @param  none
   * @return none
   */
-  //@TODO Add support for 40x4 dual controller    
-void TextLCD_Base::clrIcon() {
-  // Icons are defined by a byte bitpattern. The P0..P5 form the Icon pattern for KS0073, and P0..P4 for KS0078
-  //     P7 P6 P5 P4 P3 P2 P1 P0 
-  // 0   B1 B0  0  0  0  0  0  0
-  // 1   B1 B0  0  0  0  0  0  0
-  //        .............
-  // 15  B1 B0  0  0  0  0  0  0
-  //
-  // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE. 
-  //     B1 B0  Mode
-  //      0  0  No Blinking for this icon row
-  //      0  1  Enabled pixels in P5 will blink
-  //      1  x  Enabled pixels in P0..P5 will blink
-  //
-  // Note: the PCF2103 and PCF2113 use UDCs to set Icons 
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
-  //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6)  
-  // Note: the PCF2119 uses UDCs to set Icons 
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
-  //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7)  
-  int idx;
-  
-  switch (_ctrl) {
-    case KS0073:              
-    case KS0078:                  
-       _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N RE(1) BE LP 
-                                                 // Select Extended Instruction Set
-       for (idx=0; idx<16; idx++) { 
-         _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Ext Instr Set)
-         _writeData(0x00);                       // Clear Icon pattern (Ext Instr Set)
-       }  
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Std Instruction Set
-                                                 // Select Std Instr set        
-       break; // end KS0073, KS0078      
-
-    case ST7032_3V3:
-    case ST7032_5V:
-       _writeCommand(0x20 | _function | 0x01);   // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1              
-
-       for (idx=0; idx<16; idx++) { 
-         _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Instr Set 1)
-         _writeData(0x00);                       // Clear Icon pattern (Instr Set 1)
-       }  
-
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
-                                                 // Select Std Instr set, Select IS=0        
-       break; // end ST7032
-
-    case ST7036_3V3:
-    case ST7036_5V:
-       _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH IS2,IS1 = 01 (Select Instr Set = 1)    
-
-       for (idx=0; idx<16; idx++) { 
-         _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Instr Set 1)
-         _writeData(0x00);                       // Clear Icon pattern (Instr Set 1)
-       }  
-
-       _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instr Set = 0)    
-                                                 // Select Std Instr set, Select IS=0        
-       break; // end ST7036
-
-    case SSD1803_3V3:                  
-//    case SSD1803_5V:                      
-       _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS 
-                                                 // Select Instruction Set 1
-       for (idx=0; idx<16; idx++) { 
-         _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Ext Instr Set)
-         _writeData(0x00);                       // Clear Icon pattern (Ext Instr Set)
-       }  
-       _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS
-                                                 // Select IS=0        
-       break; // end SSD1803      
-
-     case PCF2103_3V3:
-     case PCF2113_3V3:         
-       // PCF2103 and PCF2113 use part of the UDC RAM to control Icons   
-       // Select CG RAM
-
-       _writeCommand(0x40 | (0 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
-       // Store UDC/Icon pattern: 
-       //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
-       for (int i=0; i<(3 * 8); i++) {
+//@TODO Add support for 40x4 dual controller
+void TextLCD_Base::clrIcon()
+{
+    // Icons are defined by a byte bitpattern. The P0..P5 form the Icon pattern for KS0073, and P0..P4 for KS0078
+    //     P7 P6 P5 P4 P3 P2 P1 P0
+    // 0   B1 B0  0  0  0  0  0  0
+    // 1   B1 B0  0  0  0  0  0  0
+    //        .............
+    // 15  B1 B0  0  0  0  0  0  0
+    //
+    // Bit 6 and Bit 7 in the pattern will control the blinking mode when Blink is enabled through BE.
+    //     B1 B0  Mode
+    //      0  0  No Blinking for this icon row
+    //      0  1  Enabled pixels in P5 will blink
+    //      1  x  Enabled pixels in P0..P5 will blink
+    //
+    // Note: the PCF2103 and PCF2113 use UDCs to set Icons
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Normal pattern (UDC 0..2) and
+    //   3 x 8 rows x 5 bits = 120 bits Icons for Blink pattern (UDC 4..6)
+    // Note: the PCF2119 uses UDCs to set Icons
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Normal pattern (UDC 0..3) and
+    //   4 x 8 rows x 5 bits = 160 bits Icons for Blink pattern (UDC 4..7)
+    int idx;
+
+    switch (_ctrl) {
+        case KS0073:
+        case KS0078:
+            _writeCommand(0x20 | _function_1);        // Set function, 0 0 1 DL N RE(1) BE LP
+            // Select Extended Instruction Set
+            for (idx=0; idx<16; idx++) {
+                _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Ext Instr Set)
+                _writeData(0x00);                       // Clear Icon pattern (Ext Instr Set)
+            }
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Std Instruction Set
+            // Select Std Instr set
+            break; // end KS0073, KS0078
+
+        case ST7032_3V3:
+        case ST7032_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function,  0 0 1 DL N F 0 IS=1 Select Instr Set = 1
+
+            for (idx=0; idx<16; idx++) {
+                _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Instr Set 1)
+                _writeData(0x00);                       // Clear Icon pattern (Instr Set 1)
+            }
+
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N RE(0) DH REV Select Instruction Set 0
+            // Select Std Instr set, Select IS=0
+            break; // end ST7032
+
+        case ST7036_3V3:
+        case ST7036_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH IS2,IS1 = 01 (Select Instr Set = 1)
+
+            for (idx=0; idx<16; idx++) {
+                _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Instr Set 1)
+                _writeData(0x00);                       // Clear Icon pattern (Instr Set 1)
+            }
+
+            _writeCommand(0x20 | _function);          // Set function, IS2,IS1 = 00 (Select Instr Set = 0)
+            // Select Std Instr set, Select IS=0
+            break; // end ST7036
+
+        case SSD1803_3V3:
+//    case SSD1803_5V:
+            _writeCommand(0x20 | _function | 0x01);   // Set function, 0 0 1 DL N DH RE(0) IS
+            // Select Instruction Set 1
+            for (idx=0; idx<16; idx++) {
+                _writeCommand(0x40 | idx);              // Set Icon Address, mask Address to valid range (Ext Instr Set)
+                _writeData(0x00);                       // Clear Icon pattern (Ext Instr Set)
+            }
+            _writeCommand(0x20 | _function);          // Set function, 0 0 1 DL N DH RE(0) IS
+            // Select IS=0
+            break; // end SSD1803
+
+        case PCF2103_3V3:
+        case PCF2113_3V3:
+            // PCF2103 and PCF2113 use part of the UDC RAM to control Icons
+            // Select CG RAM
+
+            _writeCommand(0x40 | (0 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
+            // Store UDC/Icon pattern:
+            //   3 x 8 rows x 5 bits = 120 bits for Normal pattern (UDC 0..2) and
+            for (int i=0; i<(3 * 8); i++) {
 //       _writeData(0x1F);  // All On
-         _writeData(0x00);  // All Off            
-       }
-
-       _writeCommand(0x40 | (4 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
-       //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6) 
-       for (int i=0; i<(3 * 8); i++) {
+                _writeData(0x00);  // All Off
+            }
+
+            _writeCommand(0x40 | (4 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
+            //   3 x 8 rows x 5 bits = 120 bits for Blink pattern (UDC 4..6)
+            for (int i=0; i<(3 * 8); i++) {
 //       _writeData(0x1F);  // All On
-         _writeData(0x00);  // All Off            
-       }
-       break; // case PCF2103_3V3 Controller
-
-     case PCF2119_3V3:  
-     case PCF2119R_3V3:
-       // PCF2119 uses part of the UDC RAM to control Icons   
-       // Select CG RAM
-
-       _writeCommand(0x40 | (0 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
-       // Store UDC/Icon pattern: 
-       //   4 x 8 rows x 5 bits = 160 bits for Normal pattern (UDC 0..3) and
-       for (int i=0; i<(4 * 8); i++) {
+                _writeData(0x00);  // All Off
+            }
+            break; // case PCF2103_3V3 Controller
+
+        case PCF2119_3V3:
+        case PCF2119R_3V3:
+            // PCF2119 uses part of the UDC RAM to control Icons
+            // Select CG RAM
+
+            _writeCommand(0x40 | (0 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
+            // Store UDC/Icon pattern:
+            //   4 x 8 rows x 5 bits = 160 bits for Normal pattern (UDC 0..3) and
+            for (int i=0; i<(4 * 8); i++) {
 //       _writeData(0x1F);  // All On
-         _writeData(0x00);  // All Off            
-       }
-
-       _writeCommand(0x40 | (4 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
-       //   4 x 8 rows x 5 bits = 160 bits for Blink pattern (UDC 4..7) 
-       for (int i=0; i<(4 * 8); i++) {
+                _writeData(0x00);  // All Off
+            }
+
+            _writeCommand(0x40 | (4 * 8)); //Set CG-RAM address, 8 sequential locations needed per UDC
+            //   4 x 8 rows x 5 bits = 160 bits for Blink pattern (UDC 4..7)
+            for (int i=0; i<(4 * 8); i++) {
 //       _writeData(0x1F);  // All On
-         _writeData(0x00);  // All Off            
-       }
-       break; // case PCF2119_3V3 Controller
-
-    default:
-       break; // end default      
-  } // end switch _ctrl           
-  
-  //Select DD RAM again for current LCD controller and restore the addresspointer
-  int addr = getAddress(_column, _row);
-  _writeCommand(0x80 | addr);
+                _writeData(0x00);  // All Off
+            }
+            break; // case PCF2119_3V3 Controller
+
+        default:
+            break; // end default
+    } // end switch _ctrl
+
+    //Select DD RAM again for current LCD controller and restore the addresspointer
+    int addr = getAddress(_column, _row);
+    _writeCommand(0x80 | addr);
 } //end clrIcon()
 #endif
 
 #if(LCD_INVERT == 1)
 /** Set Invert
-  * setInvert method is supported by some compatible devices (eg KS0073) to swap between black and white 
+  * setInvert method is supported by some compatible devices (eg KS0073) to swap between black and white
   *
-  * @param bool invertOn  Invert on/off   
+  * @param bool invertOn  Invert on/off
   * @return none
   */
-//@TODO Add support for 40x4 dual controller  
-void TextLCD_Base::setInvert(bool invertOn) {
-  
-  if (invertOn) {
-    // Controllers that support Invert
-    switch (_ctrl) {   
-      case KS0073:        
-      case KS0078:        
-        _function = _function | 0x01;                                  // Enable Invert
-        _writeCommand(0x20 | _function);                               // Activate Invert (Std Instr Set)
-        break;
-      case SSD1803_3V3 :      
-//      case SSD1803_5V :                 
-      case US2066_3V3:
+//@TODO Add support for 40x4 dual controller
+void TextLCD_Base::setInvert(bool invertOn)
+{
+
+    if (invertOn) {
+        // Controllers that support Invert
+        switch (_ctrl) {
+            case KS0073:
+            case KS0078:
+                _function = _function | 0x01;                                  // Enable Invert
+                _writeCommand(0x20 | _function);                               // Activate Invert (Std Instr Set)
+                break;
+            case SSD1803_3V3 :
+//      case SSD1803_5V :
+            case US2066_3V3:
 //      case USS2066_5V:
-        _function_1 = _function_1 | 0x01;                              // Enable Invert 
-                                                                       // Set function, 0 0 1 DL N BE RE(1) REV  (SSD1803)
-                                                                       // Set function, 0 0 1 X  N BE RE(1) REV  (US2066)    
-        _writeCommand(0x20 | _function_1);                             // Activate Invert (Ext Instr Set)
-        _writeCommand(0x20 | _function);                               // Return to Std Instr Set                            
-        break;
-      default:  
-        //Unsupported feature for other controllers
-        break;              
-    } // end switch  
-  }  
-  else {
-    // Controllers that support Invert
-    switch (_ctrl) {    
-      case KS0073:
-      case KS0078:
-        _function = _function & ~0x01;                                 // Disable Invert
-        _writeCommand(0x20 | _function);                               // Disable Invert (Std Instr Set)
-        break;
-      case SSD1803_3V3 :      
-//      case SSD1803_5V :                 
-      case US2066_3V3:
+                _function_1 = _function_1 | 0x01;                              // Enable Invert
+                // Set function, 0 0 1 DL N BE RE(1) REV  (SSD1803)
+                // Set function, 0 0 1 X  N BE RE(1) REV  (US2066)
+                _writeCommand(0x20 | _function_1);                             // Activate Invert (Ext Instr Set)
+                _writeCommand(0x20 | _function);                               // Return to Std Instr Set
+                break;
+            default:
+                //Unsupported feature for other controllers
+                break;
+        } // end switch
+    } else {
+        // Controllers that support Invert
+        switch (_ctrl) {
+            case KS0073:
+            case KS0078:
+                _function = _function & ~0x01;                                 // Disable Invert
+                _writeCommand(0x20 | _function);                               // Disable Invert (Std Instr Set)
+                break;
+            case SSD1803_3V3 :
+//      case SSD1803_5V :
+            case US2066_3V3:
 //      case USS2066_5V:
-        _function_1 = _function_1 & ~0x01;                             // Disable Invert 
-                                                                       // Set function, 0 0 1 DL N BE RE(1) REV  (SSD1803)
-                                                                       // Set function, 0 0 1 X  N BE RE(1) REV  (US2066)                                                                                                                                              
-        _writeCommand(0x20 | _function_1);                             // Activate Invert (Ext Instr Set)
-        _writeCommand(0x20 | _function);                               // Return to Std Instr Set                            
-        break;
-
-      default:  
-        //Unsupported feature for other controllers
-        break;              
-    } // end switch  
-  }
+                _function_1 = _function_1 & ~0x01;                             // Disable Invert
+                // Set function, 0 0 1 DL N BE RE(1) REV  (SSD1803)
+                // Set function, 0 0 1 X  N BE RE(1) REV  (US2066)
+                _writeCommand(0x20 | _function_1);                             // Activate Invert (Ext Instr Set)
+                _writeCommand(0x20 | _function);                               // Return to Std Instr Set
+                break;
+
+            default:
+                //Unsupported feature for other controllers
+                break;
+        } // end switch
+    }
 } // end setInvert()
 #endif
 
@@ -2801,108 +2864,119 @@
  * @param e      Enable line (clock)
  * @param d4-d7  Data lines for using as a 4-bit interface
  * @param type   Sets the panel size/addressing mode (default = LCD16x2)
- * @param bl     Backlight control line (optional, default = NC)  
- * @param e2     Enable2 line (clock for second controller, LCD40x4 only) 
- * @param ctrl   LCD controller (default = HD44780)   
- */ 
+ * @param bl     Backlight control line (optional, default = NC)
+ * @param e2     Enable2 line (clock for second controller, LCD40x4 only)
+ * @param ctrl   LCD controller (default = HD44780)
+ */
 TextLCD::TextLCD(PinName rs, PinName e,
                  PinName d4, PinName d5, PinName d6, PinName d7,
                  LCDType type, PinName bl, PinName e2, LCDCtrl ctrl) :
-                 TextLCD_Base(type, ctrl), 
-                 _rs(rs), _e(e), _d(d4, d5, d6, d7) {
-
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  // The hardware Enable2 pin is only needed for LCD40x4. Test and make sure whether it exists or not to prevent illegal access.
-  if (e2 != NC) {
-    _e2 = new DigitalOut(e2);   //Construct new pin 
-    _e2->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Enable pin       
-    _e2 = NULL;                 //Construct dummy pin     
-  }  
-  
-   _init(_LCD_DL_4);   // Set Datalength to 4 bit for mbed bus interfaces
+    TextLCD_Base(type, ctrl),
+    _rs(rs), _e(e), _d(d4, d5, d6, d7)
+{
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    // The hardware Enable2 pin is only needed for LCD40x4. Test and make sure whether it exists or not to prevent illegal access.
+    if (e2 != NC) {
+        _e2 = new DigitalOut(e2);   //Construct new pin
+        _e2->write(0);              //Deactivate
+    } else {
+        // No Hardware Enable pin
+        _e2 = NULL;                 //Construct dummy pin
+    }
+
+    _init(_LCD_DL_4);   // Set Datalength to 4 bit for mbed bus interfaces
 }
 
 /** Destruct a TextLCD interface for using regular mbed pins
   *
   * @param  none
   * @return none
-  */ 
-TextLCD::~TextLCD() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
-   if (_e2 != NULL) {delete _e2;}  // E2 pin
+  */
+TextLCD::~TextLCD()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
+    if (_e2 != NULL) {
+        delete _e2;   // E2 pin
+    }
 }
 
 /** Set E pin (or E2 pin)
   * Used for mbed pins, I2C bus expander or SPI shiftregister
   * Default PinName value for E2 is NC, must be used as pointer to avoid issues with mbed lib and DigitalOut pins
   *   @param  value true or false
-  *   @return none 
+  *   @return none
   */
-void TextLCD::_setEnable(bool value) {
-
-  if(_ctrl_idx==_LCDCtrl_0) {
-    if (value) {
-      _e  = 1;    // Set E bit 
-    }  
-    else { 
-      _e  = 0;    // Reset E bit  
-    }  
-  }    
-  else { 
-    if (value) {
-      if (_e2 != NULL) {_e2->write(1);}  //Set E2 bit
-    }  
-    else { 
-      if (_e2 != NULL) {_e2->write(0);}  //Reset E2 bit     
-    }  
-  }    
-}    
+void TextLCD::_setEnable(bool value)
+{
+
+    if(_ctrl_idx==_LCDCtrl_0) {
+        if (value) {
+            _e  = 1;    // Set E bit
+        } else {
+            _e  = 0;    // Reset E bit
+        }
+    } else {
+        if (value) {
+            if (_e2 != NULL) {
+                _e2->write(1);   //Set E2 bit
+            }
+        } else {
+            if (_e2 != NULL) {
+                _e2->write(0);   //Reset E2 bit
+            }
+        }
+    }
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD::_setRS(bool value) {
-
-  if (value) {
-    _rs  = 1;    // Set RS bit 
-  }  
-  else  {
-    _rs  = 0;    // Reset RS bit 
-  }  
-}    
+void TextLCD::_setRS(bool value)
+{
+
+    if (value) {
+        _rs  = 1;    // Set RS bit
+    } else  {
+        _rs  = 0;    // Reset RS bit
+    }
+}
 
 /** Set BL pin
   * Used for mbed pins, I2C bus expander or SPI shiftregister
   * Default PinName value is NC, must be used as pointer to avoid issues with mbed lib and DigitalOut pins
   *   @param  value true or false
-  *   @return none  
+  *   @return none
   */
-void TextLCD::_setBL(bool value) {
-
-  if (value) {
-    if (_bl != NULL) {_bl->write(1);}  //Set BL bit
-  }  
-  else { 
-    if (_bl != NULL) {_bl->write(0);}  //Reset BL bit  
-  }  
-}    
+void TextLCD::_setBL(bool value)
+{
+
+    if (value) {
+        if (_bl != NULL) {
+            _bl->write(1);   //Set BL bit
+        }
+    } else {
+        if (_bl != NULL) {
+            _bl->write(0);   //Reset BL bit
+        }
+    }
+}
 
 // Place the 4bit data on the databus
 // Used for mbed pins, I2C bus expander or SPI shifregister
-void TextLCD::_setData(int value) {
-  _d = value & 0x0F;   // Write Databits 
-}    
+void TextLCD::_setData(int value)
+{
+    _d = value & 0x0F;   // Write Databits
+}
 
 //----------- End TextLCD ---------------
 
@@ -2914,144 +2988,144 @@
   * @param i2c             I2C Bus
   * @param deviceAddress   I2C slave address (PCF8574, PCF8574A or MCP23008, default = 0x40)
   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-  * @param ctrl            LCD controller (default = HD44780)    
+  * @param ctrl            LCD controller (default = HD44780)
   */
 TextLCD_I2C::TextLCD_I2C(I2C *i2c, char deviceAddress, LCDType type, LCDCtrl ctrl) :
-                         TextLCD_Base(type, ctrl), 
-                         _i2c(i2c){
-                              
-  _slaveAddress = deviceAddress & 0xFE;
-
-  // Setup the I2C bus
-  // The max bitrate for PCF8574 is 100kbit, the max bitrate for MCP23008 is 400kbit, 
-  _i2c->frequency(100000);
-  
+    TextLCD_Base(type, ctrl),
+    _i2c(i2c)
+{
+
+    _slaveAddress = deviceAddress & 0xFE;
+
+    // Setup the I2C bus
+    // The max bitrate for PCF8574 is 100kbit, the max bitrate for MCP23008 is 400kbit,
+    _i2c->frequency(100000);
+
 #if (MCP23008==1)
-  // MCP23008 portexpander Init
-  _writeRegister(IODIR,   0x00);  // All pins are outputs
-  _writeRegister(IPOL,    0x00);  // No reverse polarity on inputs
-  _writeRegister(GPINTEN, 0x00);  // No interrupt on change of input pins
-  _writeRegister(DEFVAL,  0x00);  // Default value to compare against for interrupts
-  _writeRegister(INTCON,  0x00);  // No interrupt on changes, compare against previous pin value 
-  _writeRegister(IOCON,   0x20);  // b1=0 - Interrupt polarity active low  
-                                  // b2=0 - Interrupt pin active driver output  
-                                  // b4=0 - Slew rate enable on SDA
-                                  // b5=0 - Auto-increment on registeraddress                                  
-                                  // b5=1 - No auto-increment on registeraddress => needed for performance improved I2C expander mode
-  _writeRegister(GPPU,    0x00);  // No Pullup 
+    // MCP23008 portexpander Init
+    _writeRegister(IODIR,   0x00);  // All pins are outputs
+    _writeRegister(IPOL,    0x00);  // No reverse polarity on inputs
+    _writeRegister(GPINTEN, 0x00);  // No interrupt on change of input pins
+    _writeRegister(DEFVAL,  0x00);  // Default value to compare against for interrupts
+    _writeRegister(INTCON,  0x00);  // No interrupt on changes, compare against previous pin value
+    _writeRegister(IOCON,   0x20);  // b1=0 - Interrupt polarity active low
+    // b2=0 - Interrupt pin active driver output
+    // b4=0 - Slew rate enable on SDA
+    // b5=0 - Auto-increment on registeraddress
+    // b5=1 - No auto-increment on registeraddress => needed for performance improved I2C expander mode
+    _writeRegister(GPPU,    0x00);  // No Pullup
 //               INTF             // Interrupt flags read (Read-Only)
-//               INTCAP           // Captured inputpins at time of interrupt (Read-Only)   
-//  _writeRegister(GPIO,    0x00);  // Output/Input pins   
-//  _writeRegister(OLAT,    0x00);  // Output Latch  
-    
-  // Init the portexpander bus
-  _lcd_bus = LCD_BUS_I2C_DEF;
-  
-  // write the new data to the portexpander
-  _writeRegister(GPIO, _lcd_bus);      
+//               INTCAP           // Captured inputpins at time of interrupt (Read-Only)
+//  _writeRegister(GPIO,    0x00);  // Output/Input pins
+//  _writeRegister(OLAT,    0x00);  // Output Latch
+
+    // Init the portexpander bus
+    _lcd_bus = LCD_BUS_I2C_DEF;
+
+    // write the new data to the portexpander
+    _writeRegister(GPIO, _lcd_bus);
 #else
-  // PCF8574 of PCF8574A portexpander
-
-  // Init the portexpander bus
-  _lcd_bus = LCD_BUS_I2C_DEF;
-
-  // write the new data to the portexpander
-  _i2c->write(_slaveAddress, &_lcd_bus, 1);    
+    // PCF8574 of PCF8574A portexpander
+
+    // Init the portexpander bus
+    _lcd_bus = LCD_BUS_I2C_DEF;
+
+    // write the new data to the portexpander
+    _i2c->write(_slaveAddress, &_lcd_bus, 1);
 #endif
 
-  _init(_LCD_DL_4);   // Set Datalength to 4 bit for all serial expander interfaces
+    _init(_LCD_DL_4);   // Set Datalength to 4 bit for all serial expander interfaces
 }
 
 // Set E bit (or E2 bit) in the databus shadowvalue
 // Used for mbed I2C bus expander
-void TextLCD_I2C::_setEnableBit(bool value) {
-
-  if(_ctrl_idx==_LCDCtrl_0) {
-    if (value) {
-      _lcd_bus |= LCD_BUS_I2C_E;     // Set E bit 
-    }  
-    else {                    
-      _lcd_bus &= ~LCD_BUS_I2C_E;    // Reset E bit                     
-    }  
-  }
-  else {
-    if (value) {
-      _lcd_bus |= LCD_BUS_I2C_E2;    // Set E2 bit 
-    }  
-    else {
-      _lcd_bus &= ~LCD_BUS_I2C_E2;   // Reset E2bit                     
-    }  
-  }    
-}    
+void TextLCD_I2C::_setEnableBit(bool value)
+{
+
+    if(_ctrl_idx==_LCDCtrl_0) {
+        if (value) {
+            _lcd_bus |= LCD_BUS_I2C_E;     // Set E bit
+        } else {
+            _lcd_bus &= ~LCD_BUS_I2C_E;    // Reset E bit
+        }
+    } else {
+        if (value) {
+            _lcd_bus |= LCD_BUS_I2C_E2;    // Set E2 bit
+        } else {
+            _lcd_bus &= ~LCD_BUS_I2C_E2;   // Reset E2bit
+        }
+    }
+}
 
 // Set E pin (or E2 pin)
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_I2C::_setEnable(bool value) {
-
-  // Place the E or E2 bit data on the databus shadowvalue
-  _setEnableBit(value);
+void TextLCD_I2C::_setEnable(bool value)
+{
+
+    // Place the E or E2 bit data on the databus shadowvalue
+    _setEnableBit(value);
 
 #if (MCP23008==1)
-  // MCP23008 portexpander
-  
-  // write the new data to the portexpander
-  _writeRegister(GPIO, _lcd_bus);      
+    // MCP23008 portexpander
+
+    // write the new data to the portexpander
+    _writeRegister(GPIO, _lcd_bus);
 #else
-  // PCF8574 of PCF8574A portexpander
-
-  // write the new data to the I2C portexpander
-  _i2c->write(_slaveAddress, &_lcd_bus, 1);    
+    // PCF8574 of PCF8574A portexpander
+
+    // write the new data to the I2C portexpander
+    _i2c->write(_slaveAddress, &_lcd_bus, 1);
 #endif
-}    
+}
 
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_I2C::_setRS(bool value) {
-
-  if (value) {
-    _lcd_bus |= LCD_BUS_I2C_RS;    // Set RS bit 
-  }  
-  else {                    
-    _lcd_bus &= ~LCD_BUS_I2C_RS;   // Reset RS bit                     
-  }
+void TextLCD_I2C::_setRS(bool value)
+{
+
+    if (value) {
+        _lcd_bus |= LCD_BUS_I2C_RS;    // Set RS bit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_RS;   // Reset RS bit
+    }
 
 #if (MCP23008==1)
-  // MCP23008 portexpander
-  
-  // write the new data to the portexpander
-  _writeRegister(GPIO, _lcd_bus);      
+    // MCP23008 portexpander
+
+    // write the new data to the portexpander
+    _writeRegister(GPIO, _lcd_bus);
 #else
-  // PCF8574 of PCF8574A portexpander
-
-  // write the new data to the I2C portexpander
-  _i2c->write(_slaveAddress, &_lcd_bus, 1);    
-#endif                  
-}    
+    // PCF8574 of PCF8574A portexpander
+
+    // write the new data to the I2C portexpander
+    _i2c->write(_slaveAddress, &_lcd_bus, 1);
+#endif
+}
 
 // Set BL pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_I2C::_setBL(bool value) {
-
-  if (value) {
-    _lcd_bus |= LCD_BUS_I2C_BL;    // Set BL bit 
-  }  
-  else {                    
-    _lcd_bus &= ~LCD_BUS_I2C_BL;   // Reset BL bit                     
-  }
-  
+void TextLCD_I2C::_setBL(bool value)
+{
+
+    if (value) {
+        _lcd_bus |= LCD_BUS_I2C_BL;    // Set BL bit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_BL;   // Reset BL bit
+    }
+
 #if (MCP23008==1)
-  // MCP23008 portexpander
-  
-  // write the new data to the portexpander
-  _writeRegister(GPIO, _lcd_bus);      
+    // MCP23008 portexpander
+
+    // write the new data to the portexpander
+    _writeRegister(GPIO, _lcd_bus);
 #else
-  // PCF8574 of PCF8574A portexpander
-
-  // write the new data to the I2C portexpander
-  _i2c->write(_slaveAddress, &_lcd_bus, 1);    
-#endif                 
-}    
+    // PCF8574 of PCF8574A portexpander
+
+    // write the new data to the I2C portexpander
+    _i2c->write(_slaveAddress, &_lcd_bus, 1);
+#endif
+}
 
 #if(0)
 // New optimized v018
@@ -3059,31 +3133,32 @@
 // Place the 4bit data in the databus shadowvalue
 // Used for mbed I2C bus expander
 const char _LCD_DATA_BITS[16] = {
-      0x00,
-      (                                                   LCD_BUS_I2C_D4),
-      (                                  LCD_BUS_I2C_D5                 ),
-      (                                  LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),
-      (                 LCD_BUS_I2C_D6                                  ),                  
-      (                 LCD_BUS_I2C_D6                  | LCD_BUS_I2C_D4),
-      (                 LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5                 ),
-      (                 LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),
-      (LCD_BUS_I2C_D7                                                   ),
-      (LCD_BUS_I2C_D7                                   | LCD_BUS_I2C_D4),
-      (LCD_BUS_I2C_D7                  | LCD_BUS_I2C_D5                 ),
-      (LCD_BUS_I2C_D7                  | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),                  
-      (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6                                  ),
-      (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6                  | LCD_BUS_I2C_D4),
-      (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5                 ),
-      (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4)
-    };
-void TextLCD_I2C::_setDataBits(int value) {
-
-  //Clear all databits
-  _lcd_bus &= ~LCD_BUS_I2C_MSK;
-
-  // Set bit by bit to support any mapping of expander portpins to LCD pins 
-  _lcd_bus |= _LCD_DATA_BITS[value & 0x0F];
-}    
+    0x00,
+    (                                                   LCD_BUS_I2C_D4),
+    (                                  LCD_BUS_I2C_D5                 ),
+    (                                  LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),
+    (                 LCD_BUS_I2C_D6                                  ),
+    (                 LCD_BUS_I2C_D6                  | LCD_BUS_I2C_D4),
+    (                 LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5                 ),
+    (                 LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),
+    (LCD_BUS_I2C_D7                                                   ),
+    (LCD_BUS_I2C_D7                                   | LCD_BUS_I2C_D4),
+    (LCD_BUS_I2C_D7                  | LCD_BUS_I2C_D5                 ),
+    (LCD_BUS_I2C_D7                  | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4),
+    (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6                                  ),
+    (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6                  | LCD_BUS_I2C_D4),
+    (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5                 ),
+    (LCD_BUS_I2C_D7 | LCD_BUS_I2C_D6 | LCD_BUS_I2C_D5 | LCD_BUS_I2C_D4)
+};
+void TextLCD_I2C::_setDataBits(int value)
+{
+
+    //Clear all databits
+    _lcd_bus &= ~LCD_BUS_I2C_MSK;
+
+    // Set bit by bit to support any mapping of expander portpins to LCD pins
+    _lcd_bus |= _LCD_DATA_BITS[value & 0x0F];
+}
 #endif
 
 #if(0)
@@ -3091,37 +3166,34 @@
 // Test faster _writeByte 0.11s vs 0.27s for a 20x4 fillscreen (PCF8574)
 // Place the 4bit data in the databus shadowvalue
 // Used for mbed I2C bus expander
-void TextLCD_I2C::_setDataBits(int value) {
-
-  // Set bit by bit to support any mapping of expander portpins to LCD pins 
-  if (value & 0x01){
-    _lcd_bus |= LCD_BUS_I2C_D4;   // Set Databit 
-  }  
-  else { 
-    _lcd_bus &= ~LCD_BUS_I2C_D4;  // Reset Databit
-  }  
-
-  if (value & 0x02){
-    _lcd_bus |= LCD_BUS_I2C_D5;   // Set Databit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_I2C_D5;  // Reset Databit
-  }  
-
-  if (value & 0x04) {
-    _lcd_bus |= LCD_BUS_I2C_D6;   // Set Databit 
-  }  
-  else {                    
-    _lcd_bus &= ~LCD_BUS_I2C_D6;  // Reset Databit
-  }  
-
-  if (value & 0x08) {
-    _lcd_bus |= LCD_BUS_I2C_D7;   // Set Databit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_I2C_D7;  // Reset Databit
-  }                                      
-}    
+void TextLCD_I2C::_setDataBits(int value)
+{
+
+    // Set bit by bit to support any mapping of expander portpins to LCD pins
+    if (value & 0x01) {
+        _lcd_bus |= LCD_BUS_I2C_D4;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_D4;  // Reset Databit
+    }
+
+    if (value & 0x02) {
+        _lcd_bus |= LCD_BUS_I2C_D5;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_D5;  // Reset Databit
+    }
+
+    if (value & 0x04) {
+        _lcd_bus |= LCD_BUS_I2C_D6;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_D6;  // Reset Databit
+    }
+
+    if (value & 0x08) {
+        _lcd_bus |= LCD_BUS_I2C_D7;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_I2C_D7;  // Reset Databit
+    }
+}
 #endif
 
 #if(1)
@@ -3129,57 +3201,60 @@
 // Test faster _writeByte 0.11s vs 0.27s for a 20x4 fillscreen (PCF8574)
 // Place the 4bit data in the databus shadowvalue
 // Used for mbed I2C bus expander
-void TextLCD_I2C::_setDataBits(int value) {
-
-  //Clear all databits
-  _lcd_bus &= ~LCD_BUS_I2C_MSK;
-
-  // Set bit by bit to support any mapping of expander portpins to LCD pins 
-  if (value & 0x01){
-    _lcd_bus |= LCD_BUS_I2C_D4;   // Set Databit 
-  }  
-
-  if (value & 0x02){
-    _lcd_bus |= LCD_BUS_I2C_D5;   // Set Databit 
-  }  
-
-  if (value & 0x04) {
-    _lcd_bus |= LCD_BUS_I2C_D6;   // Set Databit 
-  }  
-
-  if (value & 0x08) {
-    _lcd_bus |= LCD_BUS_I2C_D7;   // Set Databit 
-  }  
-}    
+void TextLCD_I2C::_setDataBits(int value)
+{
+
+    //Clear all databits
+    _lcd_bus &= ~LCD_BUS_I2C_MSK;
+
+    // Set bit by bit to support any mapping of expander portpins to LCD pins
+    if (value & 0x01) {
+        _lcd_bus |= LCD_BUS_I2C_D4;   // Set Databit
+    }
+
+    if (value & 0x02) {
+        _lcd_bus |= LCD_BUS_I2C_D5;   // Set Databit
+    }
+
+    if (value & 0x04) {
+        _lcd_bus |= LCD_BUS_I2C_D6;   // Set Databit
+    }
+
+    if (value & 0x08) {
+        _lcd_bus |= LCD_BUS_I2C_D7;   // Set Databit
+    }
+}
 #endif
 
 // Place the 4bit data on the databus
 // Used for mbed pins, I2C bus expander or SPI shifregister
-void TextLCD_I2C::_setData(int value) {
-
-  // Place the 4bit data on the databus shadowvalue
-  _setDataBits(value); 
-  
-  // Place the 4bit data on the databus
+void TextLCD_I2C::_setData(int value)
+{
+
+    // Place the 4bit data on the databus shadowvalue
+    _setDataBits(value);
+
+    // Place the 4bit data on the databus
 #if (MCP23008==1)
-  // MCP23008 portexpander
-  
-  // write the new data to the portexpander
-  _writeRegister(GPIO, _lcd_bus);      
+    // MCP23008 portexpander
+
+    // write the new data to the portexpander
+    _writeRegister(GPIO, _lcd_bus);
 #else
-  // PCF8574 of PCF8574A portexpander
-
-  // write the new data to the I2C portexpander
-  _i2c->write(_slaveAddress, &_lcd_bus, 1);    
-#endif                 
-}    
+    // PCF8574 of PCF8574A portexpander
+
+    // write the new data to the I2C portexpander
+    _i2c->write(_slaveAddress, &_lcd_bus, 1);
+#endif
+}
 
 // Write data to MCP23008 I2C portexpander
 // Used for mbed I2C bus expander
-void TextLCD_I2C::_writeRegister (int reg, int value) {
-  char data[] = {reg, value};
-    
-  _i2c->write(_slaveAddress, data, 2); 
+void TextLCD_I2C::_writeRegister (int reg, int value)
+{
+    char data[] = {reg, value};
+
+    _i2c->write(_slaveAddress, data, 2);
 }
 
 //New optimized
@@ -3187,50 +3262,51 @@
 //Test faster _writeByte 0.14s vs 0.34s for a 20x4 fillscreen (MCP23008)
 
 // Write a byte using I2C
-void TextLCD_I2C::_writeByte(int value) {
-  char data[6];
-  
+void TextLCD_I2C::_writeByte(int value)
+{
+    char data[6];
+
 #if (MCP23008==1)
-  // MCP23008 portexpander
-
-  data[0] = GPIO;                 // set registeraddres
-                                  // Note: auto-increment is disabled so all data will go to GPIO register
-  
-  _setEnableBit(true);            // set E 
-  _setDataBits(value >> 4);       // set data high  
-  data[1] = _lcd_bus;
-  
-  _setEnableBit(false);           // clear E   
-  data[2] = _lcd_bus;
-  
-  _setEnableBit(true);            // set E   
-  _setDataBits(value);            // set data low    
-  data[3] = _lcd_bus;
-  
-  _setEnableBit(false);           // clear E     
-  data[4] = _lcd_bus;
-  
-  // write the packed data to the I2C portexpander
-  _i2c->write(_slaveAddress, data, 5);    
+    // MCP23008 portexpander
+
+    data[0] = GPIO;                 // set registeraddres
+    // Note: auto-increment is disabled so all data will go to GPIO register
+
+    _setEnableBit(true);            // set E
+    _setDataBits(value >> 4);       // set data high
+    data[1] = _lcd_bus;
+
+    _setEnableBit(false);           // clear E
+    data[2] = _lcd_bus;
+
+    _setEnableBit(true);            // set E
+    _setDataBits(value);            // set data low
+    data[3] = _lcd_bus;
+
+    _setEnableBit(false);           // clear E
+    data[4] = _lcd_bus;
+
+    // write the packed data to the I2C portexpander
+    _i2c->write(_slaveAddress, data, 5);
 #else
-  // PCF8574 of PCF8574A portexpander
-  
-  _setEnableBit(true);            // set E 
-  _setDataBits(value >> 4);       // set data high  
-  data[0] = _lcd_bus;
-  
-  _setEnableBit(false);           // clear E   
-  data[1] = _lcd_bus;
-  
-  _setEnableBit(true);            // set E   
-  _setDataBits(value);            // set data low    
-  data[2] = _lcd_bus;
-  
-  _setEnableBit(false);           // clear E     
-  data[3] = _lcd_bus;
-  
-  // write the packed data to the I2C portexpander
-  _i2c->write(_slaveAddress, data, 4);    
+    // PCF8574 of PCF8574A portexpander
+
+    _setEnableBit(true);            // set E
+    _setDataBits(value >> 4);       // set data high
+    data[0] = _lcd_bus;
+
+    _setEnableBit(false);           // clear E
+    data[1] = _lcd_bus;
+
+    _setEnableBit(true);            // set E
+    _setDataBits(value);            // set data low
+    data[2] = _lcd_bus;
+
+    _setEnableBit(false);           // clear E
+    data[3] = _lcd_bus;
+
+    // write the packed data to the I2C portexpander
+    _i2c->write(_slaveAddress, data, 4);
 #endif
 }
 
@@ -3241,138 +3317,134 @@
 //--------- Start TextLCD_SPI -----------
 #if(LCD_SPI == 1) /* SPI Expander SN74595          */
 
- /** Create a TextLCD interface using an SPI 74595 portexpander
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param ctrl            LCD controller (default = HD44780)      
-   */
+/** Create a TextLCD interface using an SPI 74595 portexpander
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param ctrl            LCD controller (default = HD44780)
+  */
 TextLCD_SPI::TextLCD_SPI(SPI *spi, PinName cs, LCDType type, LCDCtrl ctrl) :
-                         TextLCD_Base(type, ctrl), 
-                         _spi(spi),        
-                         _cs(cs) {              
-  // Init cs
-  _cs = 1;  
-
-  // Setup the spi for 8 bit data, low steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(8,0);
-  _spi->frequency(500000);    
-  //_spi.frequency(1000000);    
-
-  wait_ms(100);                   // Wait 100ms to ensure LCD powered up
-  
-  // Init the portexpander bus
-  _lcd_bus = LCD_BUS_SPI_DEF;
-  
-  // write the new data to the portexpander
-  _cs = 0;  
-  _spi->write(_lcd_bus);   
-  _cs = 1;  
-
-  _init(_LCD_DL_4);   // Set Datalength to 4 bit for all serial expander interfaces
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+    // Init cs
+    _cs = 1;
+
+    // Setup the spi for 8 bit data, low steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(8,0);
+    _spi->frequency(500000);
+    //_spi.frequency(1000000);
+
+    wait_ms(100);                   // Wait 100ms to ensure LCD powered up
+
+    // Init the portexpander bus
+    _lcd_bus = LCD_BUS_SPI_DEF;
+
+    // write the new data to the portexpander
+    _cs = 0;
+    _spi->write(_lcd_bus);
+    _cs = 1;
+
+    _init(_LCD_DL_4);   // Set Datalength to 4 bit for all serial expander interfaces
 }
 
 // Set E pin (or E2 pin)
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI::_setEnable(bool value) {
-
-  if(_ctrl_idx==_LCDCtrl_0) {
-    if (value) {
-      _lcd_bus |= LCD_BUS_SPI_E;     // Set E bit 
-    }  
-    else {                    
-      _lcd_bus &= ~LCD_BUS_SPI_E;    // Reset E bit                     
-    }  
-  }
-  else {
-    if (value) {
-      _lcd_bus |= LCD_BUS_SPI_E2;    // Set E2 bit 
-    }  
-    else {
-      _lcd_bus &= ~LCD_BUS_SPI_E2;   // Reset E2 bit                     
-    }  
-  }
-                  
-  // write the new data to the SPI portexpander
-  _cs = 0;  
-  _spi->write(_lcd_bus);   
-  _cs = 1;    
-}    
+void TextLCD_SPI::_setEnable(bool value)
+{
+
+    if(_ctrl_idx==_LCDCtrl_0) {
+        if (value) {
+            _lcd_bus |= LCD_BUS_SPI_E;     // Set E bit
+        } else {
+            _lcd_bus &= ~LCD_BUS_SPI_E;    // Reset E bit
+        }
+    } else {
+        if (value) {
+            _lcd_bus |= LCD_BUS_SPI_E2;    // Set E2 bit
+        } else {
+            _lcd_bus &= ~LCD_BUS_SPI_E2;   // Reset E2 bit
+        }
+    }
+
+    // write the new data to the SPI portexpander
+    _cs = 0;
+    _spi->write(_lcd_bus);
+    _cs = 1;
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister and SPI_N
-void TextLCD_SPI::_setRS(bool value) {
-
-  if (value) {
-    _lcd_bus |= LCD_BUS_SPI_RS;    // Set RS bit 
-  }  
-  else {                    
-    _lcd_bus &= ~LCD_BUS_SPI_RS;   // Reset RS bit                     
-  }
-     
-  // write the new data to the SPI portexpander
-  _cs = 0;  
-  _spi->write(_lcd_bus);   
-  _cs = 1;     
-}    
+void TextLCD_SPI::_setRS(bool value)
+{
+
+    if (value) {
+        _lcd_bus |= LCD_BUS_SPI_RS;    // Set RS bit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_RS;   // Reset RS bit
+    }
+
+    // write the new data to the SPI portexpander
+    _cs = 0;
+    _spi->write(_lcd_bus);
+    _cs = 1;
+}
 
 // Set BL pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI::_setBL(bool value) {
-
-  if (value) {
-    _lcd_bus |= LCD_BUS_SPI_BL;    // Set BL bit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_SPI_BL;   // Reset BL bit                     
-  }
-      
-  // write the new data to the SPI portexpander
-  _cs = 0;  
-  _spi->write(_lcd_bus);   
-  _cs = 1;      
-}    
+void TextLCD_SPI::_setBL(bool value)
+{
+
+    if (value) {
+        _lcd_bus |= LCD_BUS_SPI_BL;    // Set BL bit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_BL;   // Reset BL bit
+    }
+
+    // write the new data to the SPI portexpander
+    _cs = 0;
+    _spi->write(_lcd_bus);
+    _cs = 1;
+}
 
 // Place the 4bit data on the databus
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI::_setData(int value) {
-
-  // Set bit by bit to support any mapping of expander portpins to LCD pins
-  if (value & 0x01) {
-    _lcd_bus |= LCD_BUS_SPI_D4;   // Set Databit 
-  }  
-  else {                    
-    _lcd_bus &= ~LCD_BUS_SPI_D4;  // Reset Databit                     
-  }
-  
-  if (value & 0x02) {
-    _lcd_bus |= LCD_BUS_SPI_D5;   // Set Databit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_SPI_D5;  // Reset Databit                     
-  }
-  
-  if (value & 0x04) {
-    _lcd_bus |= LCD_BUS_SPI_D6;   // Set Databit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_SPI_D6;  // Reset Databit                     
-  }
-  
-  if (value & 0x08) {
-    _lcd_bus |= LCD_BUS_SPI_D7;   // Set Databit 
-  }  
-  else {
-    _lcd_bus &= ~LCD_BUS_SPI_D7;  // Reset Databit
-  }  
-                    
-  // write the new data to the SPI portexpander
-  _cs = 0;  
-  _spi->write(_lcd_bus);   
-  _cs = 1;       
-}    
+void TextLCD_SPI::_setData(int value)
+{
+
+    // Set bit by bit to support any mapping of expander portpins to LCD pins
+    if (value & 0x01) {
+        _lcd_bus |= LCD_BUS_SPI_D4;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_D4;  // Reset Databit
+    }
+
+    if (value & 0x02) {
+        _lcd_bus |= LCD_BUS_SPI_D5;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_D5;  // Reset Databit
+    }
+
+    if (value & 0x04) {
+        _lcd_bus |= LCD_BUS_SPI_D6;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_D6;  // Reset Databit
+    }
+
+    if (value & 0x08) {
+        _lcd_bus |= LCD_BUS_SPI_D7;   // Set Databit
+    } else {
+        _lcd_bus &= ~LCD_BUS_SPI_D7;  // Reset Databit
+    }
+
+    // write the new data to the SPI portexpander
+    _cs = 0;
+    _spi->write(_lcd_bus);
+    _cs = 1;
+}
 
 #endif /* SPI Expander SN74595          */
 //---------- End TextLCD_SPI ------------
@@ -3381,110 +3453,116 @@
 //--------- Start TextLCD_I2C_N ---------
 #if(LCD_I2C_N == 1)  /* Native I2C */
 
- /** Create a TextLCD interface using a controller with native I2C interface
-   *
-   * @param i2c             I2C Bus
-   * @param deviceAddress   I2C slave address (default = 0x7C)  
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)     
-   * @param ctrl            LCD controller (default = ST7032_3V3)                     
-   */
-TextLCD_I2C_N::TextLCD_I2C_N(I2C *i2c, char deviceAddress, LCDType type, PinName bl, LCDCtrl ctrl) : 
-                               TextLCD_Base(type, ctrl), 
-
-                               _i2c(i2c){
-  
-  _slaveAddress = deviceAddress & 0xFE;
-  
-  // Setup the I2C bus
-  // The max bitrate for ST7032i is 400kbit, lets stick to default here
-  _i2c->frequency(100000);
-
-       
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-  
-  //Sanity check
-  if (_ctrl & LCD_C_I2C) {
-    _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces      
-  }
-  else {
-    error("Error: LCD Controller type does not support native I2C interface\n\r");           
-  }
+/** Create a TextLCD interface using a controller with native I2C interface
+  *
+  * @param i2c             I2C Bus
+  * @param deviceAddress   I2C slave address (default = 0x7C)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = ST7032_3V3)
+  */
+TextLCD_I2C_N::TextLCD_I2C_N(I2C *i2c, char deviceAddress, LCDType type, PinName bl, LCDCtrl ctrl) :
+    TextLCD_Base(type, ctrl),
+
+    _i2c(i2c)
+{
+
+    _slaveAddress = deviceAddress & 0xFE;
+
+    // Setup the I2C bus
+    // The max bitrate for ST7032i is 400kbit, lets stick to default here
+    _i2c->frequency(100000);
+
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_I2C) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native I2C interface\n\r");
+    }
 }
 
-TextLCD_I2C_N::~TextLCD_I2C_N() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_I2C_N::~TextLCD_I2C_N()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_I2C_N::_setEnable(bool value) {
-}    
+void TextLCD_I2C_N::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister and native I2C or SPI
-void TextLCD_I2C_N::_setRS(bool value) {
+void TextLCD_I2C_N::_setRS(bool value)
+{
 // The controlbyte defines the meaning of the next byte. This next byte can either be data or command.
 // Start Slaveaddress+RW  b7 b6 b5 b4 b3 b2 b1 b0   b7...........b0  Stop
 //                        Co RS RW  0  0  0  0  0   command or data
 //
-//   C0=1 indicates that another controlbyte will follow after the next data or command byte 
+//   C0=1 indicates that another controlbyte will follow after the next data or command byte
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
-//   RW=0 means write to controller. RW=1 means that controller will be read from after the next command. 
-//        Many native I2C controllers dont support this option and it is not used by this lib. 
+//   RW=0 means write to controller. RW=1 means that controller will be read from after the next command.
+//        Many native I2C controllers dont support this option and it is not used by this lib.
 //
 
-  if (value) {
-    _controlbyte = 0x40; // Next byte is data, No more control bytes will follow
-  }
-  else {
-    _controlbyte = 0x00; // Next byte is command, No more control bytes will follow     
-  }
-}    
+    if (value) {
+        _controlbyte = 0x40; // Next byte is data, No more control bytes will follow
+    } else {
+        _controlbyte = 0x00; // Next byte is command, No more control bytes will follow
+    }
+}
 
 // Set BL pin
-void TextLCD_I2C_N::_setBL(bool value) {
+void TextLCD_I2C_N::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
-    
+        _bl->write(value);
+    }
+}
+
 // Not used in this mode
-void TextLCD_I2C_N::_setData(int value) {
-}    
+void TextLCD_I2C_N::_setData(int value)
+{
+}
 
 // Write a byte using I2C
-void TextLCD_I2C_N::_writeByte(int value) {
+void TextLCD_I2C_N::_writeByte(int value)
+{
 // The controlbyte defines the meaning of the next byte. This next byte can either be data or command.
 // Start Slaveaddress+RW  b7 b6 b5 b4 b3 b2 b1 b0   b7...........b0  Stop
 //                        Co RS RW  0  0  0  0  0   command or data
 //
-//   C0=1 indicates that another controlbyte will follow after the next data or command byte 
+//   C0=1 indicates that another controlbyte will follow after the next data or command byte
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
-//   RW=0 means write to controller. RW=1 means that controller will be read from after the next command. 
-//        Many native I2C controllers dont support this option and it is not used by this lib. 
+//   RW=0 means write to controller. RW=1 means that controller will be read from after the next command.
+//        Many native I2C controllers dont support this option and it is not used by this lib.
 //
-  char data[] = {_controlbyte, value};
-    
+    char data[] = {_controlbyte, value};
+
 #if(LCD_I2C_ACK==1)
 //Controllers that support ACK
-  _i2c->write(_slaveAddress, data, 2); 
-#else  
+    _i2c->write(_slaveAddress, data, 2);
+#else
 //Controllers that dont support ACK
 //Note: This may be issue with some mbed platforms that dont fully/correctly support I2C byte operations.
-  _i2c->start(); 
-  _i2c->write(_slaveAddress);   
-  _i2c->write(data[0]); 
-  _i2c->write(data[1]);     
-  _i2c->stop();   
-#endif  
+    _i2c->start();
+    _i2c->write(_slaveAddress);
+    _i2c->write(data[0]);
+    _i2c->write(data[1]);
+    _i2c->stop();
+#endif
 }
 #endif /* Native I2C */
 //-------- End TextLCD_I2C_N ------------
@@ -3492,224 +3570,236 @@
 
 //--------- Start TextLCD_SPI_N ---------
 #if(LCD_SPI_N == 1) /* Native SPI bus     */
- /** Create a TextLCD interface using a controller with a native SPI4 interface
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param rs              Instruction/data control line
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = ST7032_3V3) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI4 interface
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param rs              Instruction/data control line
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = ST7032_3V3)
+  */
 TextLCD_SPI_N::TextLCD_SPI_N(SPI *spi, PinName cs, PinName rs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                             TextLCD_Base(type, ctrl), 
-                             _spi(spi),        
-                             _cs(cs),
-                             _rs(rs) {      
-        
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 8 bit data, high steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate    
-//  _spi->format(8,3);  
-//  _spi->frequency(500000); 
-//  _spi->frequency(1000000);    
-  
-  // Setup the spi for 8 bit data, low steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(8,0);
-//  _spi->frequency(500000); 
-  _spi->frequency(1000000);    
-    
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI4) {
-    _init(_LCD_DL_8);   // Set Datalength to 8 bit for all native serial interfaces
-                        // ST7070 must set datalength to 8 bits!
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI4 interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs),
+    _rs(rs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 8 bit data, high steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+//  _spi->format(8,3);
+//  _spi->frequency(500000);
+//  _spi->frequency(1000000);
+
+    // Setup the spi for 8 bit data, low steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(8,0);
+//  _spi->frequency(500000);
+    _spi->frequency(1000000);
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI4) {
+        _init(_LCD_DL_8);   // Set Datalength to 8 bit for all native serial interfaces
+        // ST7070 must set datalength to 8 bits!
+    } else {
+        error("Error: LCD Controller type does not support native SPI4 interface\n\r");
+    }
 }
 
-TextLCD_SPI_N::~TextLCD_SPI_N() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N::~TextLCD_SPI_N()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister, SPI_N
-void TextLCD_SPI_N::_setRS(bool value) {
+void TextLCD_SPI_N::_setRS(bool value)
+{
     _rs = value;
-}    
+}
 
 // Set BL pin
-void TextLCD_SPI_N::_setBL(bool value) {
+void TextLCD_SPI_N::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N::_setData(int value) {
-}    
+void TextLCD_SPI_N::_setData(int value)
+{
+}
 
 // Write a byte using SPI
-void TextLCD_SPI_N::_writeByte(int value) {
+void TextLCD_SPI_N::_writeByte(int value)
+{
     _cs = 0;
     wait_us(1);
     _spi->write(value);
     wait_us(1);
     _cs = 1;
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //-------- End TextLCD_SPI_N ------------
 
 
 //-------- Start TextLCD_SPI_N_3_8 --------
 #if(LCD_SPI_N_3_8 == 1) /* Native SPI bus     */
 
- /** Create a TextLCD interface using a controller with a native SPI3 8 bits interface
-   * This mode is supported by ST7070. Note that implementation in TexTLCD is not very efficient due to
-   * structure of the TextLCD library: each databyte is written separately and requires a separate 'count command' set to 1 byte.
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = ST7070) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI3 8 bits interface
+  * This mode is supported by ST7070. Note that implementation in TexTLCD is not very efficient due to
+  * structure of the TextLCD library: each databyte is written separately and requires a separate 'count command' set to 1 byte.
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = ST7070)
+  */
 TextLCD_SPI_N_3_8::TextLCD_SPI_N_3_8(SPI *spi, PinName cs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                                     TextLCD_Base(type, ctrl), 
-                                     _spi(spi),        
-                                     _cs(cs) {      
-
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 8 bit data, high steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-//  _spi->format(8,3);  
-//  _spi->frequency(500000);    
-//  _spi->frequency(1000000);    
-
-  // Setup the spi for 8 bit data, low steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(8,0);
-//  _spi->frequency(500000); 
-  _spi->frequency(1000000);    
-  
-  
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI3_8) { 
-    _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces   
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI3 8 bits interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 8 bit data, high steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+//  _spi->format(8,3);
+//  _spi->frequency(500000);
+//  _spi->frequency(1000000);
+
+    // Setup the spi for 8 bit data, low steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(8,0);
+//  _spi->frequency(500000);
+    _spi->frequency(1000000);
+
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI3_8) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native SPI3 8 bits interface\n\r");
+    }
 }
 
-TextLCD_SPI_N_3_8::~TextLCD_SPI_N_3_8() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N_3_8::~TextLCD_SPI_N_3_8()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N_3_8::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N_3_8::_setEnable(bool value)
+{
+}
 
 // Used for mbed pins, I2C bus expander or SPI shiftregister, SPI_N
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
-void TextLCD_SPI_N_3_8::_setRS(bool value) {
-  
-  if (value) {
-    _controlbyte = 0x01; // Next byte is data, No more control bytes will follow
-  }
-  else {
-    _controlbyte = 0x00; // Next byte is command, No more control bytes will follow     
-  }
-}      
-  
+void TextLCD_SPI_N_3_8::_setRS(bool value)
+{
+
+    if (value) {
+        _controlbyte = 0x01; // Next byte is data, No more control bytes will follow
+    } else {
+        _controlbyte = 0x00; // Next byte is command, No more control bytes will follow
+    }
+}
+
 // Set BL pin
-void TextLCD_SPI_N_3_8::_setBL(bool value) {
+void TextLCD_SPI_N_3_8::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N_3_8::_setData(int value) {
-}    
+void TextLCD_SPI_N_3_8::_setData(int value)
+{
+}
 
 // Write a byte using SPI3 8 bits mode (ST7070)
-void TextLCD_SPI_N_3_8::_writeByte(int value) {
-    
-  if (_controlbyte == 0x00) { // Byte is command 
-    _cs = 0;
-    wait_us(1);
-    _spi->write(value);
-    wait_us(1);
-    _cs = 1;
-  }  
-  else {                      // Byte is data 
-    // Select Extended Instr Set
-    _cs = 0;
-    wait_us(1);
-    _spi->write(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1));
-    wait_us(1);
-    _cs = 1;     
-
-    wait_us(40);                            // Wait until command has finished...    
-        
-    // Set Count to 1 databyte
-    _cs = 0;
-    wait_us(1);    
-    _spi->write(0x80);                      // Set display data length, 1 L6 L5 L4 L3 L2 L1 L0 (Instr Set = 1)
-    wait_us(1);
-    _cs = 1;
-
-    wait_us(40);    
-                
-    // Write 1 databyte     
-    _cs = 0;
-    wait_us(1);    
-    _spi->write(value);                     // Write data (Instr Set = 1)
-    wait_us(1);
-    _cs = 1;         
-
-    wait_us(40);    
-        
-    // Select Standard Instr Set    
-    _cs = 0;
-    wait_us(1);    
-    _spi->write(0x20 | _function);          // Set function, 0 0 1 DL N EXT=0 x x (Select Instr Set = 0));
-    wait_us(1);
-    _cs = 1;     
-  }  
+void TextLCD_SPI_N_3_8::_writeByte(int value)
+{
+
+    if (_controlbyte == 0x00) { // Byte is command
+        _cs = 0;
+        wait_us(1);
+        _spi->write(value);
+        wait_us(1);
+        _cs = 1;
+    } else {                    // Byte is data
+        // Select Extended Instr Set
+        _cs = 0;
+        wait_us(1);
+        _spi->write(0x20 | _function | 0x04);   // Set function, 0 0 1 DL N EXT=1 x x (Select Instr Set = 1));
+        wait_us(1);
+        _cs = 1;
+
+        wait_us(40);                            // Wait until command has finished...
+
+        // Set Count to 1 databyte
+        _cs = 0;
+        wait_us(1);
+        _spi->write(0x80);                      // Set display data length, 1 L6 L5 L4 L3 L2 L1 L0 (Instr Set = 1)
+        wait_us(1);
+        _cs = 1;
+
+        wait_us(40);
+
+        // Write 1 databyte
+        _cs = 0;
+        wait_us(1);
+        _spi->write(value);                     // Write data (Instr Set = 1)
+        wait_us(1);
+        _cs = 1;
+
+        wait_us(40);
+
+        // Select Standard Instr Set
+        _cs = 0;
+        wait_us(1);
+        _spi->write(0x20 | _function);          // Set function, 0 0 1 DL N EXT=0 x x (Select Instr Set = 0));
+        wait_us(1);
+        _cs = 1;
+    }
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //------- End TextLCD_SPI_N_3_8 -----------
 
 
@@ -3717,343 +3807,365 @@
 #if(LCD_SPI_N_3_9 == 1) /* Native SPI bus     */
 //Code checked out on logic analyser. Not yet tested on hardware..
 
- /** Create a TextLCD interface using a controller with a native SPI3 9 bits interface
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = AIP31068) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI3 9 bits interface
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = AIP31068)
+  */
 TextLCD_SPI_N_3_9::TextLCD_SPI_N_3_9(SPI *spi, PinName cs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                                     TextLCD_Base(type, ctrl), 
-                                     _spi(spi),        
-                                     _cs(cs) {      
-
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 9 bit data, high steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(9,3);  
-  _spi->frequency(1000000);    
-  
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI3_9) { 
-    _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces   
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI3 9 bits interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 9 bit data, high steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(9,3);
+    _spi->frequency(1000000);
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI3_9) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native SPI3 9 bits interface\n\r");
+    }
 }
 
-TextLCD_SPI_N_3_9::~TextLCD_SPI_N_3_9() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N_3_9::~TextLCD_SPI_N_3_9()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N_3_9::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N_3_9::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI_N_3_9::_setRS(bool value) {
+void TextLCD_SPI_N_3_9::_setRS(bool value)
+{
 // The controlbits define the meaning of the next byte. This next byte can either be data or command.
-//   b8  b7...........b0 
+//   b8  b7...........b0
 //   RS  command or data
 //
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
 //
 
-  if (value) {
-    _controlbyte = 0x01; // Next byte is data
-  }
-  else {
-    _controlbyte = 0x00; // Next byte is command
-  }  
-}    
+    if (value) {
+        _controlbyte = 0x01; // Next byte is data
+    } else {
+        _controlbyte = 0x00; // Next byte is command
+    }
+}
 
 // Set BL pin
-void TextLCD_SPI_N_3_9::_setBL(bool value) {
+void TextLCD_SPI_N_3_9::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N_3_9::_setData(int value) {
-}    
+void TextLCD_SPI_N_3_9::_setData(int value)
+{
+}
 
 // Write a byte using SPI3 9 bits mode
-void TextLCD_SPI_N_3_9::_writeByte(int value) {
+void TextLCD_SPI_N_3_9::_writeByte(int value)
+{
     _cs = 0;
     wait_us(1);
     _spi->write( (_controlbyte << 8) | (value & 0xFF));
     wait_us(1);
     _cs = 1;
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //------- End TextLCD_SPI_N_3_9 -----------
 
 
 //------- Start TextLCD_SPI_N_3_10 --------
 #if(LCD_SPI_N_3_10 == 1) /* Native SPI bus     */
 
- /** Create a TextLCD interface using a controller with a native SPI3 10 bits interface
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = AIP31068) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI3 10 bits interface
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = AIP31068)
+  */
 TextLCD_SPI_N_3_10::TextLCD_SPI_N_3_10(SPI *spi, PinName cs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                                       TextLCD_Base(type, ctrl), 
-                                       _spi(spi),        
-                                       _cs(cs) {      
-        
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 10 bit data, low steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(10,0);
-  _spi->frequency(1000000);    
-  
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI3_10) {
-     _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces            
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI3 10 bits interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 10 bit data, low steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(10,0);
+    _spi->frequency(1000000);
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI3_10) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native SPI3 10 bits interface\n\r");
+    }
 }
 
-TextLCD_SPI_N_3_10::~TextLCD_SPI_N_3_10() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N_3_10::~TextLCD_SPI_N_3_10()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N_3_10::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N_3_10::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI_N_3_10::_setRS(bool value) {
+void TextLCD_SPI_N_3_10::_setRS(bool value)
+{
 // The controlbits define the meaning of the next byte. This next byte can either be data or command.
-//   b9 b8  b7...........b0 
+//   b9 b8  b7...........b0
 //   RS RW  command or data
 //
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
 //   RW=0 means that next byte is writen, RW=1 means that next byte is read (not used in this lib)
 //
 
-  if (value) {
-    _controlbyte = 0x02; // Next byte is data
-  }
-  else {
-    _controlbyte = 0x00; // Next byte is command
-  }  
-}    
+    if (value) {
+        _controlbyte = 0x02; // Next byte is data
+    } else {
+        _controlbyte = 0x00; // Next byte is command
+    }
+}
 
 // Set BL pin
-void TextLCD_SPI_N_3_10::_setBL(bool value) {
+void TextLCD_SPI_N_3_10::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N_3_10::_setData(int value) {
-}    
+void TextLCD_SPI_N_3_10::_setData(int value)
+{
+}
 
 // Write a byte using SPI3 10 bits mode
-void TextLCD_SPI_N_3_10::_writeByte(int value) {
+void TextLCD_SPI_N_3_10::_writeByte(int value)
+{
     _cs = 0;
     wait_us(1);
     _spi->write( (_controlbyte << 8) | (value & 0xFF));
     wait_us(1);
     _cs = 1;
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //------- End TextLCD_SPI_N_3_10 ----------
 
 
 //------- Start TextLCD_SPI_N_3_16 --------
 #if(LCD_SPI_N_3_16 == 1) /* Native SPI bus     */
 
- /** Create a TextLCD interface using a controller with a native SPI3 16 bits interface
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = PT6314) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI3 16 bits interface
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = PT6314)
+  */
 TextLCD_SPI_N_3_16::TextLCD_SPI_N_3_16(SPI *spi, PinName cs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                                       TextLCD_Base(type, ctrl), 
-                                       _spi(spi),        
-                                       _cs(cs) {      
-        
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 8 bit data, low steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(8,0);
-  _spi->frequency(1000000);    
-  
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI3_16) {
-     _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces            
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI3 16 bits interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 8 bit data, low steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(8,0);
+    _spi->frequency(1000000);
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI3_16) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native SPI3 16 bits interface\n\r");
+    }
 }
 
-TextLCD_SPI_N_3_16::~TextLCD_SPI_N_3_16() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N_3_16::~TextLCD_SPI_N_3_16()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N_3_16::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N_3_16::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI_N_3_16::_setRS(bool value) {
+void TextLCD_SPI_N_3_16::_setRS(bool value)
+{
 // The 16bit mode is split in 2 bytes. The first byte is for synchronisation and controlbits. The controlbits define the meaning of the next byte.
 // The 8 actual bits represent either a data or a command byte.
-//   b15 b14 b13 b12 b11 b10  b9  b8 - b7 b6 b5 b4 b3 b2 b1 b0 
+//   b15 b14 b13 b12 b11 b10  b9  b8 - b7 b6 b5 b4 b3 b2 b1 b0
 //     1   1   1   1   1  RW  RS   0   d7 d6 d5 d4 d3 d2 d1 d0
 //
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
 //   RW=0 means that next byte is writen, RW=1 means that next byte is read (not used in this lib)
 //
 
-  if (value) {
-    _controlbyte = 0xFA; // Next byte is data
-  }
-  else {
-    _controlbyte = 0xF8; // Next byte is command
-  }  
-}    
+    if (value) {
+        _controlbyte = 0xFA; // Next byte is data
+    } else {
+        _controlbyte = 0xF8; // Next byte is command
+    }
+}
 
 // Set BL pin
-void TextLCD_SPI_N_3_16::_setBL(bool value) {
+void TextLCD_SPI_N_3_16::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N_3_16::_setData(int value) {
-}    
-   
+void TextLCD_SPI_N_3_16::_setData(int value)
+{
+}
+
 // Write a byte using SPI3 16 bits mode
-void TextLCD_SPI_N_3_16::_writeByte(int value) {
+void TextLCD_SPI_N_3_16::_writeByte(int value)
+{
     _cs = 0;
     wait_us(1);
 
     _spi->write(_controlbyte);
 
-    _spi->write(value);     
+    _spi->write(value);
 
     wait_us(1);
     _cs = 1;
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //------- End TextLCD_SPI_N_3_16 ----------
 
 
 //------- Start TextLCD_SPI_N_3_24 --------
 #if(LCD_SPI_N_3_24 == 1) /* Native SPI bus     */
 
- /** Create a TextLCD interface using a controller with a native SPI3 24 bits interface
-   *
-   * @param spi             SPI Bus
-   * @param cs              chip select pin (active low)
-   * @param type            Sets the panel size/addressing mode (default = LCD16x2)
-   * @param bl              Backlight control line (optional, default = NC)  
-   * @param ctrl            LCD controller (default = SSD1803) 
-   */       
+/** Create a TextLCD interface using a controller with a native SPI3 24 bits interface
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  * @param bl              Backlight control line (optional, default = NC)
+  * @param ctrl            LCD controller (default = SSD1803)
+  */
 TextLCD_SPI_N_3_24::TextLCD_SPI_N_3_24(SPI *spi, PinName cs, LCDType type, PinName bl, LCDCtrl ctrl) :
-                                       TextLCD_Base(type, ctrl), 
-                                       _spi(spi),        
-                                       _cs(cs) {      
-        
-  // Init CS
-  _cs = 1;
-
-  // Setup the spi for 8 bit data, high steady state clock,
-  // rising edge capture, with a 500KHz or 1MHz clock rate  
-  _spi->format(8,3);
-  _spi->frequency(1000000);    
-  
-  // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
-  if (bl != NC) {
-    _bl = new DigitalOut(bl);   //Construct new pin 
-    _bl->write(0);              //Deactivate    
-  }
-  else {
-    // No Hardware Backlight pin       
-    _bl = NULL;                 //Construct dummy pin     
-  }  
-
-  //Sanity check
-  if (_ctrl & LCD_C_SPI3_24) {
-    _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces      
-  }
-  else {
-    error("Error: LCD Controller type does not support native SPI3 24 bits interface\n\r");           
-  }
+    TextLCD_Base(type, ctrl),
+    _spi(spi),
+    _cs(cs)
+{
+
+    // Init CS
+    _cs = 1;
+
+    // Setup the spi for 8 bit data, high steady state clock,
+    // rising edge capture, with a 500KHz or 1MHz clock rate
+    _spi->format(8,3);
+    _spi->frequency(1000000);
+
+    // The hardware Backlight pin is optional. Test and make sure whether it exists or not to prevent illegal access.
+    if (bl != NC) {
+        _bl = new DigitalOut(bl);   //Construct new pin
+        _bl->write(0);              //Deactivate
+    } else {
+        // No Hardware Backlight pin
+        _bl = NULL;                 //Construct dummy pin
+    }
+
+    //Sanity check
+    if (_ctrl & LCD_C_SPI3_24) {
+        _init(_LCD_DL_8);  // Set Datalength to 8 bit for all native serial interfaces
+    } else {
+        error("Error: LCD Controller type does not support native SPI3 24 bits interface\n\r");
+    }
 }
 
-TextLCD_SPI_N_3_24::~TextLCD_SPI_N_3_24() {
-   if (_bl != NULL) {delete _bl;}  // BL pin
+TextLCD_SPI_N_3_24::~TextLCD_SPI_N_3_24()
+{
+    if (_bl != NULL) {
+        delete _bl;   // BL pin
+    }
 }
 
 // Not used in this mode
-void TextLCD_SPI_N_3_24::_setEnable(bool value) {
-}    
+void TextLCD_SPI_N_3_24::_setEnable(bool value)
+{
+}
 
 // Set RS pin
 // Used for mbed pins, I2C bus expander or SPI shiftregister
-void TextLCD_SPI_N_3_24::_setRS(bool value) {
+void TextLCD_SPI_N_3_24::_setRS(bool value)
+{
 // The 24bit mode is split in 3 bytes. The first byte is for synchronisation and controlbits. The controlbits define the meaning of the next two bytes.
 // Each byte encodes 4 actual bits. The 8 actual bits represent either a data or a command byte.
-//   b23 b22 b21 b20 b19 b18 b17 b16 -  b15 b14 b13 b12 b11 b10 b9 b8 - b7 b6 b5 b4 b3 b2 b1 b0 
+//   b23 b22 b21 b20 b19 b18 b17 b16 -  b15 b14 b13 b12 b11 b10 b9 b8 - b7 b6 b5 b4 b3 b2 b1 b0
 //     1   1   1   1   1  RW  RS   0     d0  d1  d2  d3   0   0  0  0   d4 d5 d6 d7  0  0  0  0
 //
 //   RS=1 means that next byte is data, RS=0 means that next byte is command
@@ -4061,42 +4173,44 @@
 //
 // Note: SPI3_24 expects LSB first. This is inconsistent with regular SPI convention (and hardware) that sends MSB first.
 
-  if (value) {
-    _controlbyte = 0xFA; // Next byte is data
-  }
-  else {
-    _controlbyte = 0xF8; // Next byte is command
-  }   
-}    
+    if (value) {
+        _controlbyte = 0xFA; // Next byte is data
+    } else {
+        _controlbyte = 0xF8; // Next byte is command
+    }
+}
 
 // Set BL pin
-void TextLCD_SPI_N_3_24::_setBL(bool value) {
+void TextLCD_SPI_N_3_24::_setBL(bool value)
+{
     if (_bl) {
-        _bl->write(value);   
-    }    
-}    
+        _bl->write(value);
+    }
+}
 
 // Not used in this mode
-void TextLCD_SPI_N_3_24::_setData(int value) {
-}    
+void TextLCD_SPI_N_3_24::_setData(int value)
+{
+}
 
 //Mapping table to flip the bits around cause SPI3_24 expects LSB first.
 const uint8_t map3_24[16] = {0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0};
-    
+
 // Write a byte using SPI3 24 bits mode
-void TextLCD_SPI_N_3_24::_writeByte(int value) {
+void TextLCD_SPI_N_3_24::_writeByte(int value)
+{
     _cs = 0;
     wait_us(1);
     _spi->write(_controlbyte);
 
     //Map and send the LSB nibble
-    _spi->write( map3_24[value & 0x0F]);     
+    _spi->write( map3_24[value & 0x0F]);
 
     //Map and send the MSB nibble
-    _spi->write( map3_24[(value >> 4) & 0x0F]);     
+    _spi->write( map3_24[(value >> 4) & 0x0F]);
 
     wait_us(1);
     _cs = 1;
 }
-#endif /* Native SPI bus     */  
+#endif /* Native SPI bus     */
 //------- End TextLCD_SPI_N_3_24 ----------