add comment Pololu TB9051FTG

add Pololu TB9051FTG
only set TCCR1A register for pins that are signal pins, done by calling setRailcomCutout() at right place instead of doing it for all pins in the timer code
2025-07-31 11:23:44 +02:00 · 2022-07-04 22:50:19 +02:00 · 2022-07-04 22:42:04 +02:00 · 2022-07-03 10:22:30 +02:00 · 2022-05-17 08:06:10 +02:00 · 2022-05-15 01:44:37 +02:00
44 changed files with 916 additions and 1352 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,4 @@ Release/*
 config.h
 .vscode/extensions.json
 mySetup.h
+myFilter.cpp
--- a/CommandStation-EX.ino
+++ b/CommandStation-EX.ino
@@ -27,8 +27,7 @@
 /*
 *  © 2020,2021 Chris Harlow, Harald Barth, David Cutting, 
 *  Fred Decker, Gregor Baues, Anthony W - Dayton  All rights reserved.
- *  
- *  This file is part of CommandStation-EX
+ * 
 *
 *  This is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
@@ -59,28 +58,19 @@ void setup()
  // Responsibility 1: Start the usb connection for diagnostics
  // This is normally Serial but uses SerialUSB on a SAMD processor
  Serial.begin(115200);
-#ifdef ESP_DEBUG
-  Serial.setDebugOutput(true);
-#endif
-
-  DIAG(F("License GPLv3 fsf.org (c) dcc-ex.com"));
-
+   
  CONDITIONAL_LCD_START {
    // This block is still executed for DIAGS if LCD not in use 
    LCD(0,F("DCC++ EX v%S"),F(VERSION));
-    LCD(1,F("Lic GPLv3")); 
+    LCD(1,F("Starting")); 
    }   

-  // Responsibility 2: Start all the communications before the DCC engine
-  // Start the WiFi interface on a MEGA, Uno cannot currently handle WiFi
-  // Start Ethernet if it exists
+//  Start the WiFi interface on a MEGA, Uno cannot currently handle WiFi
+
 #if WIFI_ON
-#ifndef ESP_FAMILY
  WifiInterface::setup(WIFI_SERIAL_LINK_SPEED, F(WIFI_SSID), F(WIFI_PASSWORD), F(WIFI_HOSTNAME), IP_PORT, WIFI_CHANNEL);
-#else
-  WifiESP::setup(WIFI_SSID, WIFI_PASSWORD, WIFI_HOSTNAME, IP_PORT, WIFI_CHANNEL);
-#endif
 #endif // WIFI_ON
+
 #if ETHERNET_ON
  EthernetInterface::setup();
 #endif // ETHERNET_ON
@@ -89,9 +79,12 @@ void setup()
  // Note: this provides DCC with two motor drivers, main and prog, which handle the motor shield(s)
  // Standard supported devices have pre-configured macros but custome hardware installations require
  //  detailed pin mappings and may also require modified subclasses of the MotorDriver to implement specialist logic.
-  // STANDARD_MOTOR_SHIELD, POLOLU_MOTOR_SHIELD, FIREBOX_MK1, FIREBOX_MK1S are pre defined in MotorShields.h
-  DCC::begin(MOTOR_SHIELD_TYPE);

+  // STANDARD_MOTOR_SHIELD, POLOLU_MOTOR_SHIELD, FIREBOX_MK1, FIREBOX_MK1S are pre defined in MotorShields.h
+
+ 
+  DCC::begin(MOTOR_SHIELD_TYPE); 
+         
  #if defined(RMFT_ACTIVE) 
      RMFT::begin();
  #endif
@@ -117,17 +110,14 @@ void loop()
  // Responsibility 1: Handle DCC background processes
  //                   (loco reminders and power checks)
  DCC::loop();
+
  // Responsibility 2: handle any incoming commands on USB connection
  serialParser.loop(Serial);

 // Responsibility 3: Optionally handle any incoming WiFi traffic
 #if WIFI_ON
-#ifndef ESP_FAMILY
  WifiInterface::loop();
-#else
-  WifiESP::loop();
 #endif
-#endif //WIFI_ON
 #if ETHERNET_ON
  EthernetInterface::loop();
 #endif
@@ -144,9 +134,7 @@ void loop()
  
  // Report any decrease in memory (will automatically trigger on first call)
  static int ramLowWatermark = __INT_MAX__; // replaced on first loop 
-#ifdef ESP_FAMILY
-  updateMinimumFreeMemory(128);
-#endif
+
  int freeNow = minimumFreeMemory();
  if (freeNow < ramLowWatermark)
  {
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -304,14 +304,14 @@ const ackOp FLASH WRITE_BIT0_PROG[] = {
     W0,WACK,
     V0, WACK,  // validate bit is 0 
     ITC1,      // if acked, callback(1)
-     CALLFAIL  // callback (-1)
+     FAIL  // callback (-1)
 };
 const ackOp FLASH WRITE_BIT1_PROG[] = {
     BASELINE,
     W1,WACK,
     V1, WACK,  // validate bit is 1 
     ITC1,      // if acked, callback(1)
-     CALLFAIL  // callback (-1)
+     FAIL  // callback (-1)
 };

 const ackOp FLASH VERIFY_BIT0_PROG[] = {
@@ -320,7 +320,7 @@ const ackOp FLASH VERIFY_BIT0_PROG[] = {
     ITC0,      // if acked, callback(0)
     V1, WACK,  // validate bit is 1
     ITC1,       
-     CALLFAIL  // callback (-1)
+     FAIL  // callback (-1)
 };
 const ackOp FLASH VERIFY_BIT1_PROG[] = {
     BASELINE,
@@ -328,7 +328,7 @@ const ackOp FLASH VERIFY_BIT1_PROG[] = {
     ITC1,      // if acked, callback(1)
     V0, WACK, 
     ITC0,
-     CALLFAIL  // callback (-1)
+     FAIL  // callback (-1)
 };

 const ackOp FLASH READ_BIT_PROG[] = {
@@ -337,7 +337,7 @@ const ackOp FLASH READ_BIT_PROG[] = {
     ITC1,      // if acked, callback(1)
     V0, WACK,  // validate bit is zero
     ITC0,      // if acked callback 0
-     CALLFAIL       // bit not readable 
+     FAIL       // bit not readable 
     };
     
 const ackOp FLASH WRITE_BYTE_PROG[] = {
@@ -345,7 +345,7 @@ const ackOp FLASH WRITE_BYTE_PROG[] = {
      WB,WACK,ITC1,    // Write and callback(1) if ACK 
      // handle decoders that dont ack a write 
      VB,WACK,ITC1,    // validate byte and callback(1) if correct 
-      CALLFAIL        // callback (-1)
+      FAIL        // callback (-1)
      };
      
 const ackOp FLASH VERIFY_BYTE_PROG[] = {
@@ -370,7 +370,7 @@ const ackOp FLASH VERIFY_BYTE_PROG[] = {
      V0, WACK, MERGE,
      V0, WACK, MERGE,
      VB, WACK, ITCB,  // verify merged byte and return it if acked ok 
-      CALLFAIL };
+      FAIL };
      
      
 const ackOp FLASH READ_CV_PROG[] = {
@@ -393,7 +393,7 @@ const ackOp FLASH READ_CV_PROG[] = {
      V0, WACK, MERGE,
      V0, WACK, MERGE,
      VB, WACK, ITCB,  // verify merged byte and return it if acked ok 
-      CALLFAIL };          // verification failed
+      FAIL };          // verification failed


 const ackOp FLASH LOCO_ID_PROG[] = {
@@ -459,7 +459,7 @@ const ackOp FLASH LOCO_ID_PROG[] = {
      V0, WACK, MERGE,
      V0, WACK, MERGE,
      VB, WACK, ITCB,  // verify merged byte and callback
-      CALLFAIL
+      FAIL
      };    

 const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
@@ -476,7 +476,7 @@ const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
      SETBYTEL,   // low byte of word 
      WB,WACK,    // some decoders don't ACK writes
      VB,WACK,ITCB,
-      CALLFAIL
+      FAIL
 };    

 const ackOp FLASH LONG_LOCO_ID_PROG[] = {
@@ -500,7 +500,7 @@ const ackOp FLASH LONG_LOCO_ID_PROG[] = {
      SETBYTEL,   // low byte of word 
      WB,WACK,
      VB,WACK,ITC1,   // callback(1) means Ok
-      CALLFAIL
+      FAIL
 };    

 void  DCC::writeCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback)  {
@@ -828,7 +828,7 @@ void DCC::ackManagerLoop() {
          }
        break;
        
-      case CALLFAIL:  // callback(-1)
+      case FAIL:  // callback(-1)
           callback(-1);
           return;
           
@@ -904,10 +904,6 @@ void DCC::callback(int value) {

    switch (callbackState) {    
       case AFTER_WRITE:  // first attempt to callback after a write operation
-	    if (!ackManagerRejoin && !DCCWaveform::progTrack.autoPowerOff) {
-               callbackState=READY;
-               break;
-            }                              // lines 906-910 added. avoid wait after write. use 1 PROG
            callbackStart=millis();
            callbackState=WAITING_100;
            if (Diag::ACK) DIAG(F("Stable 100mS"));
--- a/DCC.h
+++ b/DCC.h
@@ -40,7 +40,7 @@ enum ackOp : byte
  ITCB,             // If True callback(byte)
  ITCB7,            // If True callback(byte &0x7F)
  NAKFAIL,          // if false callback(-1)
-  CALLFAIL,         // callback(-1)
+  FAIL,             // callback(-1)
  STARTMERGE,       // Clear bit and byte settings ready for merge pass
  MERGE,            // Merge previous wack response with byte value and decrement bit number (use for readimng CV bytes)
  SETBIT,           // sets bit number to next prog byte
@@ -191,8 +191,6 @@ private:
 #define ARDUINO_TYPE "TEENSY40"
 #elif defined(ARDUINO_TEENSY41)
 #define ARDUINO_TYPE "TEENSY41"
-#elif defined(ARDUINO_ARCH_ESP8266)
-#define ARDUINO_TYPE "ESP8266"
 #else
 #error CANNOT COMPILE - DCC++ EX ONLY WORKS WITH AN ARDUINO UNO, NANO 328, OR ARDUINO MEGA 1280/2560
 #endif
--- a/DCCEX.h
+++ b/DCCEX.h
@@ -1,24 +1,3 @@
-/*
- *  (c) 2020 Chris Harlow. All rights reserved.
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  (c) 2020 Harald Barth. All rights reserved.
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-
 // This include is intended to visually simplify the .ino for the end users.
 // If there were any #ifdefs required they are much better handled in here.  

@@ -31,9 +10,6 @@
 #include "DCCEXParser.h"
 #include "version.h"
 #include "WifiInterface.h"
-#ifdef ESP_FAMILY
-#include "WifiESP.h"
-#endif
 #if ETHERNET_ON == true
 #include "EthernetInterface.h"
 #endif
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -17,7 +17,6 @@
 *  You should have received a copy of the GNU General Public License
 *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
 */
-#include "defines.h"
 #include "StringFormatter.h"
 #include "DCCEXParser.h"
 #include "DCC.h"
@@ -31,11 +30,9 @@

 #include "EEStore.h"
 #include "DIAG.h"
-#ifndef ESP_FAMILY
 #include <avr/wdt.h>
-#endif

-// These keywords are used in the <1> command. The number is what you get if you use the keyword as a parameter.
+// These keywords are used in various commands. The number is what you get if you use the keyword as a parameter.
 // To discover new keyword numbers , use the <$ YOURKEYWORD> command
 const int16_t HASH_KEYWORD_PROG = -29718;
 const int16_t HASH_KEYWORD_MAIN = 11339;
@@ -59,6 +56,7 @@ const int16_t HASH_KEYWORD_LCN = 15137;
 const int16_t HASH_KEYWORD_RESET = 26133;
 const int16_t HASH_KEYWORD_SPEED28 = -17064;
 const int16_t HASH_KEYWORD_SPEED128 = 25816;
+const int16_t HASH_KEYWORD_RAILCOM = -29097;

 int16_t DCCEXParser::stashP[MAX_COMMAND_PARAMS];
 bool DCCEXParser::stashBusy;
@@ -581,11 +579,7 @@ bool DCCEXParser::parseZ(Print *stream, int16_t params, int16_t p[])
    }
        return true;

-    case 3: // <Z ID PIN IFLAG>
-        if (p[0] < 0 ||
-	    p[1] > 255 || p[1] <= 1 || // Pins 0 and 1 are Serial to USB
-	    p[2] <   0 || p[2] > 7 )
-	  return false;
+    case 3: // <Z ID PIN INVERT>
        if (!Output::create(p[0], p[1], p[2], 1))
          return false;
        StringFormatter::send(stream, F("<O>\n"));
@@ -783,6 +777,9 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
        Diag::LCN = onOff;
        return true;

+    case HASH_KEYWORD_RAILCOM: // <D RAILCOM ON/OFF>
+        return DCCWaveform::setUseRailcom(onOff);        
+
    case HASH_KEYWORD_PROGBOOST:
        DCC::setProgTrackBoost(true);
 	      return true;
--- a/DCCTimer.cpp
+++ b/DCCTimer.cpp
@@ -44,7 +44,7 @@

 #include "DCCTimer.h"
 const int DCC_SIGNAL_TIME=58;  // this is the 58uS DCC 1-bit waveform half-cycle 
-const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME);
+const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;

 INTERRUPT_CALLBACK interruptHandler=0;

@@ -53,11 +53,11 @@ INTERRUPT_CALLBACK interruptHandler=0;
  
  void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
    interruptHandler=callback;
-    noInterrupts();
+    noInterrupts(); 
    ADC0.CTRLC = (ADC0.CTRLC & 0b00110000) | 0b01000011;  // speed up analogRead sample time   
    TCB0.CTRLB = TCB_CNTMODE_INT_gc & ~TCB_CCMPEN_bm; // timer compare mode with output disabled
    TCB0.CTRLA = TCB_CLKSEL_CLKDIV2_gc; //   8 MHz ~ 0.125 us      
-    TCB0.CCMP =  (CLOCK_CYCLES>>1) -1;  // 1 tick less for timer reset
+    TCB0.CCMP =  CLOCK_CYCLES -1;  // 1 tick less for timer reset
    TCB0.INTFLAGS = TCB_CAPT_bm; // clear interrupt request flag
    TCB0.INTCTRL = TCB_CAPT_bm;  // Enable the interrupt
    TCB0.CNT = 0;
@@ -75,6 +75,11 @@ INTERRUPT_CALLBACK interruptHandler=0;
       (void) pin; 
       return false;  // TODO what are the relevant pins? 
  }
+  
+  bool DCCTimer::isPWMPin(byte pin) {
+       (void) pin; 
+       return false;  // TODO what are the relevant pins? 
+  }

 void DCCTimer::setPWM(byte pin, bool high) {
    (void) pin;
@@ -90,24 +95,65 @@ INTERRUPT_CALLBACK interruptHandler=0;

 #elif defined(TEENSYDUINO)
  IntervalTimer myDCCTimer;
-
+  bool interruptFlipflop=false; 
+  byte railcomPin[2]={0,0];
+  enum RAILCOM_NEXT:byte {SKIP,CUT_OUT,CUT_IN);
+  RAILCOM_NEXT railcom1Next[]={SKIP,SKIP};
+  
  void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
    interruptHandler=callback;
-
-  myDCCTimer.begin(interruptHandler, DCC_SIGNAL_TIME);
-
+    myDCCTimer.begin(interruptFast, DCC_SIGNAL_TIME/2);
  }

+  // This interrupt happens every 29uS, and alternately calls the DCC waveform
+  // or handles any pending Railcom cutout pins. 
+  void interruptFast() {
+       nterruptFlipflop=!interruptFlipflop;
+       if (interruptFiliflop) {
+        interruptHandler();
+        return;
+       }
+      
+       // Railcom interrupt, half way between DCC interruots
+       for (byte channel=0;channel<2;channel++) {
+          byte pin=railcomPin[channel;
+          if (pin) {
+            switch (railcomNext[channel]) {
+              case CUT_OUT: 
+                    digitalWrite(pin,HIGH);
+                    break;
+              case CUT_IN:          
+                    digitalWrite(pin,HIGH);
+                    break;
+              case IGNORE: break;               
+            }
+            railcomNext[channel]=IGNORE;
+          }
+       }
+  }
+  
  bool DCCTimer::isPWMPin(byte pin) {
-       //Teensy: digitalPinHasPWM, todo
      (void) pin;
-       return false;  // TODO what are the relevant pins? 
+       return true;  // We are so fast we can pretend we do support this 
+  }
+
+  bool DCCTimer::isRailcomPin(byte pin) {
+      (void) pin;
+      if (railcomPin[0]==0) railcomPin[0]=pin;
+      else if (railcomPin[1]==0) railcomPin[1]=pin;
+      else return false; 
+      return true;  // We are so fast we can pretend we do support this 
  }

 void DCCTimer::setPWM(byte pin, bool high) {
-    // TODO what are the relevant pins?
-    (void) pin;
-    (void) high;
+    // setting pwm on a railcom pin is deferred to the next railcom interruyupt.
+    for (byte channel=0;channel<2;channel++) {
+      if (pin==railcomPin[channel]) {
+        railcomNext[channel]=high?CUT_OUT:CUT_IN;
+        return; 
+      }
+    }
+    digitalWrite(pin,high?HIGH:LOW);
 }

  void   DCCTimer::getSimulatedMacAddress(byte mac[6]) {
@@ -146,51 +192,40 @@ void DCCTimer::read(uint8_t word, uint8_t *mac, uint8_t offset) {
 }
 #endif

-#elif defined(ARDUINO_ARCH_ESP8266)
-
-void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
-  interruptHandler=callback;
-  timer1_disable();
-
-  // There seem to be differnt ways to attach interrupt handler
-  //    ETS_FRC_TIMER1_INTR_ATTACH(NULL, NULL);
-  //    ETS_FRC_TIMER1_NMI_INTR_ATTACH(interruptHandler);
-  // Let us choose the one from the API
-  timer1_attachInterrupt(interruptHandler);
-
-  // not exactly sure of order:
-  timer1_enable(TIM_DIV1, TIM_EDGE, TIM_LOOP);
-  timer1_write(CLOCK_CYCLES);
-}
-// We do not support to use PWM to make the Waveform on ESP
-bool IRAM_ATTR DCCTimer::isPWMPin(byte pin) {
-  return false;
-}
-void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
-}
-
-
-
 #else 
  // Arduino nano, uno, mega etc
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    #define TIMER1_A_PIN   11
    #define TIMER1_B_PIN   12
    #define TIMER1_C_PIN   13
-#else
+    //railcom timer facility
+    #define TIMER4_A_PIN   6
+    #define TIMER4_B_PIN   7
+    #define TIMER4_C_PIN   8
+ #else
   #define TIMER1_A_PIN   9
   #define TIMER1_B_PIN   10
 #endif

  void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
    interruptHandler=callback;
-    noInterrupts();
+    noInterrupts();          
    ADCSRA = (ADCSRA & 0b11111000) | 0b00000100;   // speed up analogRead sample time 
    TCCR1A = 0;
-    ICR1 = CLOCK_CYCLES>>1;
-    TCNT1 = 0;   
+    ICR1 = CLOCK_CYCLES;
    TCCR1B = _BV(WGM13) | _BV(CS10);     // Mode 8, clock select 1
    TIMSK1 = _BV(TOIE1); // Enable Software interrupt
+    TCNT1 = 0;   
+    
+#if defined(TIMER4_A_PIN)
+    //railcom timer facility
+    TCCR4A = 0;
+    ICR4 = CLOCK_CYCLES;
+    TCCR4B = _BV(WGM43) | _BV(CS40);     // Mode 8, clock select 1
+    TIMSK4 = 0; // Disable Software interrupt
+    delayMicroseconds(DCC_SIGNAL_TIME/2);  
+    TCNT4 = 0;  // this timer fires half cycle after Timer 1 (no idea why /4 !)     
+#endif
    interrupts();
  }

@@ -206,20 +241,64 @@ void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
       #endif       
       ;
  }
+// Alternative pin manipulation via PWM control.
+  bool DCCTimer::isRailcomPin(byte pin) {
+       return  
+       #ifdef TIMER4_A_PIN 
+              pin==TIMER4_A_PIN ||
+              pin==TIMER4_B_PIN ||
+              pin==TIMER4_C_PIN ||
+       #endif       
+              false;
+  }

- void DCCTimer::setPWM(byte pin, bool high) {
-    if (pin==TIMER1_A_PIN) {
+void DCCTimer::onoffPWM(byte pin, bool on) {
+  if (pin==TIMER1_A_PIN) {
+    if (on)
      TCCR1A |= _BV(COM1A1);
-      OCR1A= high?1024:0;
+    else
+      TCCR1A &= ~(_BV(COM1A1));
+  }
+  else if (pin==TIMER1_B_PIN) {
+    if (on)
+      TCCR1A |= _BV(COM1B1);
+    else
+      TCCR1A &= ~(_BV(COM1B1));
+  }
+#ifdef TIMER1_C_PIN
+  else if (pin==TIMER1_C_PIN) {
+    if (on)
+      TCCR1A |= _BV(COM1C1);
+    else
+      TCCR1A &= ~(_BV(COM1C1));
+  }
+#endif
+}
+ void DCCTimer::setPWM(byte pin, bool high) {
+    uint16_t val=high?1024:0;
+    if (pin==TIMER1_A_PIN) {
+      OCR1A= val;
    }
    else if (pin==TIMER1_B_PIN) { 
-      TCCR1A |= _BV(COM1B1);
-      OCR1B= high?1024:0;
+      OCR1B= val;
    }
 #ifdef TIMER1_C_PIN 
    else if (pin==TIMER1_C_PIN) { 
-      TCCR1A |= _BV(COM1C1);
-      OCR1C= high?1024:0;
+      OCR1C= val;
+    }
+ #endif       
+ #ifdef TIMER4_A_PIN 
+    else if (pin==TIMER4_A_PIN) { 
+      TCCR4A |= _BV(COM4A1); 
+      OCR4A= val;
+    }
+    else if (pin==TIMER4_B_PIN) { 
+      TCCR4A |= _BV(COM4B1); 
+      OCR4B= val;
+    }
+    else if (pin==TIMER4_C_PIN) { 
+      TCCR4A |= _BV(COM4C1); 
+      OCR4C= val;
    }
 #endif       
 }
--- a/DCCTimer.h
+++ b/DCCTimer.h
@@ -1,23 +1,3 @@
-/*
- *  (c) 2021 Mike S. All rights reserved.
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-
 #ifndef DCCTimer_h
 #define DCCTimer_h
 #include "Arduino.h"
@@ -29,7 +9,9 @@ class DCCTimer {
  static void begin(INTERRUPT_CALLBACK interrupt);
  static void getSimulatedMacAddress(byte mac[6]);
  static bool isPWMPin(byte pin);
+  static bool isRailcomPin(byte pin);
  static void setPWM(byte pin, bool high);
+  static void onoffPWM(byte pin, bool on);
 #if (defined(TEENSYDUINO) && !defined(__IMXRT1062__))
  static void read_mac(byte mac[6]);
  static void read(uint8_t word, uint8_t *mac, uint8_t offset);
--- a/DCCWaveform.cpp
+++ b/DCCWaveform.cpp
@@ -20,7 +20,6 @@
 #pragma GCC optimize ("-O3")
 #include <Arduino.h>

-#include "defines.h"
 #include "DCCWaveform.h"
 #include "DCCTimer.h"
 #include "DIAG.h"
@@ -29,6 +28,8 @@
 DCCWaveform  DCCWaveform::mainTrack(PREAMBLE_BITS_MAIN, true);
 DCCWaveform  DCCWaveform::progTrack(PREAMBLE_BITS_PROG, false);

+bool DCCWaveform::useRailcom=false;
+bool DCCWaveform::supportsRailcom=false;
 bool DCCWaveform::progTrackSyncMain=false; 
 bool DCCWaveform::progTrackBoosted=false; 
 int  DCCWaveform::progTripValue=0;
@@ -47,53 +48,54 @@ void DCCWaveform::begin(MotorDriver * mainDriver, MotorDriver * progDriver) {
 				 && (mainDriver->getFaultPin() != UNUSED_PIN));
  // Only use PWM if both pins are PWM capable. Otherwise JOIN does not work
  MotorDriver::usePWM= mainDriver->isPWMCapable() && progDriver->isPWMCapable();
-  if (MotorDriver::usePWM)
+  supportsRailcom= MotorDriver::usePWM && mainDriver->isRailcomCapable() && progDriver->isRailcomCapable();
+  
+  // supportsRailcom depends on hardware caopability
+  // useRailcom is user switchable at run time. 
+  useRailcom=supportsRailcom;
+  
+  if (MotorDriver::usePWM){
    DIAG(F("Signal pin config: high accuracy waveform"));
+    if (supportsRailcom) DIAG(F("Railcom cutout enabled"));
+  }
  else
    DIAG(F("Signal pin config: normal accuracy waveform"));
  DCCTimer::begin(DCCWaveform::interruptHandler);     
 }

-#ifdef SLOW_ANALOG_READ
-// Flag to hold if we need to run ack checking in loop
-static bool ackflag = 0;
-#endif
-
-void IRAM_ATTR DCCWaveform::loop(bool ackManagerActive) {
+void DCCWaveform::loop(bool ackManagerActive) {
  mainTrack.checkPowerOverload(false);
  progTrack.checkPowerOverload(ackManagerActive);
-#ifdef SLOW_ANALOG_READ
-  if (ackflag) {
-    progTrack.checkAck();
-    // reset flag AFTER check is done
-    ackflag = 0;
-  }
-#endif
 }

-void IRAM_ATTR DCCWaveform::interruptHandler() {
+void DCCWaveform::interruptHandler() {
  // call the timer edge sensitive actions for progtrack and maintrack
  // member functions would be cleaner but have more overhead
  byte sigMain=signalTransform[mainTrack.state];
  byte sigProg=progTrackSyncMain? sigMain : signalTransform[progTrack.state];
+  
  // Set the signal state for both tracks
  mainTrack.motorDriver->setSignal(sigMain);
  progTrack.motorDriver->setSignal(sigProg);
+  
  // Move on in the state engine
  mainTrack.state=stateTransform[mainTrack.state];    
  progTrack.state=stateTransform[progTrack.state];    
+
+
+  // WAVE_START is at start of bit where we need to find
+  // out if this is an railcom start or stop time
+  if (useRailcom) {
+    if (mainTrack.state==WAVE_START) mainTrack.railcom2();
+    if (progTrack.state==WAVE_START) progTrack.railcom2();
+  }
+
  // WAVE_PENDING means we dont yet know what the next bit is
-  if (mainTrack.state==WAVE_PENDING)
-    mainTrack.interrupt2();
-  if (progTrack.state==WAVE_PENDING)
-    progTrack.interrupt2();
-#ifdef SLOW_ANALOG_READ
-  else if (progTrack.ackPending && ackflag == 0) // We need AND we are not already checking
-    ackflag = 1;
-#else
-  else if (progTrack.ackPending)
-    progTrack.checkAck();
-#endif
+  // so call interrupt2 to set it
+  if (mainTrack.state==WAVE_PENDING) mainTrack.interrupt2();
+  if (progTrack.state==WAVE_PENDING) progTrack.interrupt2();
+  else if (progTrack.ackPending) progTrack.checkAck();
+
 }


@@ -132,6 +134,16 @@ void DCCWaveform::setPowerMode(POWERMODE mode) {
  motorDriver->setPower( ison);
 }

+bool DCCWaveform::setUseRailcom(bool on) {
+  if (!supportsRailcom) return false; 
+  useRailcom=on;
+  if (!on) {
+    // turn off any existing cutout 
+    mainTrack.motorDriver->setRailcomCutout(false);
+    progTrack.motorDriver->setRailcomCutout(false);
+  }
+  return true;
+}

 void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
  if (millis() - lastSampleTaken  < sampleDelay) return;
@@ -212,8 +224,28 @@ const bool DCCWaveform::signalTransform[]={
   /* WAVE_MID_0   -> */ LOW,
   /* WAVE_LOW_0   -> */ LOW,
   /* WAVE_PENDING (should not happen) -> */ LOW};
-        
-void IRAM_ATTR DCCWaveform::interrupt2() {
+
+void DCCWaveform::railcom2() {
+  bool cutout;
+  if (remainingPreambles==(requiredPreambles-2)) {
+    cutout=true;
+  } else if (remainingPreambles==(requiredPreambles-6)) {
+    cutout=false;
+  } else {
+    return; // neiter start or end of cutout, do nothing
+  }
+
+  if (isMainTrack) {
+    if (progTrackSyncMain) // we are main track and synced so we take care of prog track as well
+      progTrack.motorDriver->setRailcomCutout(cutout);
+    mainTrack.motorDriver->setRailcomCutout(cutout);
+  } else {
+    if (!progTrackSyncMain) // we are prog track and not synced so we take care of ourselves
+      progTrack.motorDriver->setRailcomCutout(cutout);
+  }
+}
+
+void DCCWaveform::interrupt2() {
  // calculate the next bit to be sent:
  // set state WAVE_MID_1  for a 1=bit
  //        or WAVE_HIGH_0 for a 0 bit.
@@ -221,11 +253,12 @@ void IRAM_ATTR DCCWaveform::interrupt2() {
  if (remainingPreambles > 0 ) {
    state=WAVE_MID_1;  // switch state to trigger LOW on next interrupt
    remainingPreambles--;
+
    // Update free memory diagnostic as we don't have anything else to do this time.
    // Allow for checkAck and its called functions using 22 bytes more.
-#ifndef ESP_FAMILY
-    updateMinimumFreeMemory(22);
-#endif
+    // Don't need to do that more than once per packet
+    if (remainingPreambles == 3)
+      updateMinimumFreeMemory(22);
    return;
  }

@@ -324,14 +357,14 @@ byte DCCWaveform::getAck() {
      return(0);  // pending set off but not detected means no ACK.   
 }

-void IRAM_ATTR DCCWaveform::checkAck() {
+void DCCWaveform::checkAck() {
    // This function operates in interrupt() time so must be fast and can't DIAG 
    if (sentResetsSincePacket > 6) {  //ACK timeout
        ackCheckDuration=millis()-ackCheckStart;
        ackPending = false;
        return; 
    }
-
+      
    int current=motorDriver->getCurrentRaw();
    numAckSamples++;
    if (current > ackMaxCurrent) ackMaxCurrent=current;
--- a/DCCWaveform.h
+++ b/DCCWaveform.h
@@ -53,10 +53,14 @@ class DCCWaveform {
    static void loop(bool ackManagerActive);
    static DCCWaveform  mainTrack;
    static DCCWaveform  progTrack;
+    static bool supportsRailcom;
+    static bool useRailcom;

    void beginTrack();
    void setPowerMode(POWERMODE);
    POWERMODE getPowerMode();
+    static bool setUseRailcom(bool on); 
+    
    void checkPowerOverload(bool ackManagerActive);
    inline int get1024Current() {
 	  if (powerMode == POWERMODE::ON)
@@ -118,6 +122,7 @@ class DCCWaveform {
  
    static void interruptHandler();
    void interrupt2();
+    void railcom2();
    void checkAck();
    
    bool isMainTrack;
--- a/DisplayInterface.cpp
+++ b/DisplayInterface.cpp
@@ -1,22 +0,0 @@
-/*
- *  © 2021, Chris Harlow, Neil McKechnie. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-#include "DisplayInterface.h"
-
-DisplayInterface *DisplayInterface::lcdDisplay = 0;
--- a/DisplayInterface.h
+++ b/DisplayInterface.h
@@ -1,35 +0,0 @@
-/*
- *  © 2021, Chris Harlow, Neil McKechnie. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-#ifndef DisplayInterface_h
-#define DisplayInterface_h
-
-#include <Arduino.h>
-
-// Definition of base class for displays.  The base class does nothing.
-class DisplayInterface : public Print {
-public:
-  virtual DisplayInterface* loop2(bool force) { (void)force; return NULL; };
-  virtual void setRow(byte line) { (void)line; };
-  virtual void clear() {};
-  virtual size_t write(uint8_t c) { (void)c; return 0; };
-
-  static DisplayInterface *lcdDisplay;
-};
-
-#endif
--- a/EEStore.h
+++ b/EEStore.h
@@ -1,22 +1,3 @@
-/*
- *  (c) 2020 Chris Harlow. All rights reserved.
- *  (c) 2020 Harald Barth. All rights reserved.
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 #ifndef EEStore_h
 #define EEStore_h

@@ -33,9 +14,9 @@ extern ExternalEEPROM EEPROM;

 struct EEStoreData{
  char id[sizeof(EESTORE_ID)];
-  uint16_t nTurnouts;
-  uint16_t nSensors;
-  uint16_t nOutputs;
+  int nTurnouts;
+  int nSensors;  
+  int nOutputs;
 };

 struct EEStore{
--- a/EthernetInterface.cpp
+++ b/EthernetInterface.cpp
@@ -68,16 +68,6 @@ EthernetInterface::EthernetInterface()
      DIAG(F("Ethernet shield not found"));
      return;
    }
-  
-    unsigned long startmilli = millis();
-    while ((millis() - startmilli) < 5500) // Loop to give time to check for cable connection
-    {
-        if (Ethernet.linkStatus() == LinkON)
-            break;
-        DIAG(F("Ethernet waiting for link (1sec) "));
-        delay(1000);
-    }
-
    if (Ethernet.linkStatus() == LinkOFF) {
      DIAG(F("Ethernet cable not connected"));
      return;
--- a/FSH.h
+++ b/FSH.h
@@ -1,21 +1,3 @@
-/*
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 #ifndef FSH_h
 #define FSH_h

@@ -29,7 +11,6 @@
 *  __FlashStringHelper     Use FSH instead.
 *  PROGMEM                 use FLASH instead
 *  pgm_read_byte_near      use GETFLASH instead.
- *  pgm_read_word_near      use GETFLASHW instead.
 *  
 */
 #include <Arduino.h>
@@ -40,14 +21,10 @@
 #define F(str) (str)
 typedef char FSH; 
 #define GETFLASH(addr) (*(const unsigned char *)(addr))
-#define GETFLASHW(addr) (*(const unsigned short *)(addr))
 #define FLASH
-#define strlen_P strlen
-#define strcpy_P strcpy
 #else 
 typedef __FlashStringHelper FSH;
 #define GETFLASH(addr) pgm_read_byte_near(addr)
-#define GETFLASHW(addr) pgm_read_word_near(addr)
 #define FLASH PROGMEM
 #endif
 #endif
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "50fcbc0"
+#define GITHUB_SHA "90487d2"
--- a/LCDDisplay.cpp
+++ b/LCDDisplay.cpp
@@ -73,7 +73,6 @@ void LCDDisplay::loop() {

 LCDDisplay *LCDDisplay::loop2(bool force) {
  if (!lcdDisplay) return NULL;
-
  unsigned long currentMillis = millis();

  if (!force) {
@@ -160,4 +159,4 @@ void LCDDisplay::moveToNextRow() {
 void LCDDisplay::skipBlankRows() {
  while (!done && rowBuffer[rowNext][0] == 0)
    moveToNextRow();
-}
+}
--- a/LCDDisplay.h
+++ b/LCDDisplay.h
@@ -19,7 +19,6 @@
 #ifndef LCDDisplay_h
 #define LCDDisplay_h
 #include <Arduino.h>
-#include "DisplayInterface.h"

 #if __has_include ( "config.h")
  #include "config.h"
@@ -30,44 +29,43 @@
 #define MAX_MSG_SIZE 16
 #endif

-// Set default scroll mode (overridable in config.h)
-#if !defined(SCROLLMODE) 
-#define SCROLLMODE 1
-#endif
-
 // This class is created in LCDisplay_Implementation.h

-class LCDDisplay : public DisplayInterface {
+class LCDDisplay : public Print {
 public:
  static const int MAX_LCD_ROWS = 8;
  static const int MAX_LCD_COLS = MAX_MSG_SIZE;
  static const long LCD_SCROLL_TIME = 3000;  // 3 seconds

+  static LCDDisplay* lcdDisplay;
+  LCDDisplay();
+  void interfake(int p1, int p2, int p3);
+
  // Internally handled functions
  static void loop();
  LCDDisplay* loop2(bool force);
  void setRow(byte line);
  void clear();

-  size_t write(uint8_t b);
-
-protected:
-  uint8_t lcdRows;
-  uint8_t lcdCols;
+  virtual size_t write(uint8_t b);
+  using Print::write;

 private:
  void moveToNextRow();
  void skipBlankRows();

-  // Relay functions to the live driver in the subclass
-  virtual void clearNative() = 0;
-  virtual void setRowNative(byte line) = 0;
-  virtual size_t writeNative(uint8_t b) = 0;
+  // Relay functions to the live driver
+  void clearNative();
+  void displayNative();
+  void setRowNative(byte line);
+  void writeNative(char b);

  unsigned long lastScrollTime = 0;
  int8_t hotRow = 0;
  int8_t hotCol = 0;
  int8_t topRow = 0;
+  uint8_t lcdRows;
+  uint8_t lcdCols;
  int8_t slot = 0;
  int8_t rowFirst = -1;
  int8_t rowNext = 0;
--- a/LCD_Implementation.h
+++ b/LCD_Implementation.h
@@ -27,27 +27,29 @@

 #ifndef LCD_Implementation_h
 #define LCD_Implementation_h
+#include <Wire.h>
 #include "LCDDisplay.h"
-#include "SSD1306Ascii.h"
-#include "LiquidCrystal_I2C.h"
  
+LCDDisplay * LCDDisplay::lcdDisplay=0;

 // Implement the LCDDisplay shim class as a singleton.
-// The DisplayInterface class implements a displayy handler with no code (null device);
-// The LCDDisplay class sub-classes DisplayInterface to provide the common display code;
-// Then LCDDisplay class is subclassed to the specific device type classes:
-//    SSD1306AsciiWire for I2C OLED driver with SSD1306 or SH1106 controllers;
-//    LiquidCrystal_I2C for I2C LCD driver for HD44780 with PCF8574 'backpack'.
-
-#if defined(OLED_DRIVER)
-  #define CONDITIONAL_LCD_START for (DisplayInterface * dummy=new SSD1306AsciiWire(OLED_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))
+// Notice that the LCDDisplay class declaration (LCDDisplay.h) is independent of the library
+// but the implementation is compiled here with dependencies on LCDDriver which is 
+// specific to the library in use.
+// Thats the workaround to the drivers not all implementing a common interface. 
+ 
+#if defined(OLED_DRIVER) 
+  #include "LCD_OLED.h"
+  #define CONDITIONAL_LCD_START for (LCDDisplay * dummy=new LCDDisplay();dummy!=NULL; dummy=dummy->loop2(true)) 
  
-#elif defined(LCD_DRIVER)
-  #define CONDITIONAL_LCD_START for (DisplayInterface * dummy=new LiquidCrystal_I2C(LCD_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))

-#else
-  // Create null display handler just in case someone calls lcdDisplay->something without checking if lcdDisplay is NULL!
-  #define CONDITIONAL_LCD_START { new DisplayInterface(); }
+#elif defined(LCD_DRIVER)  
+  #include "LCD_LCD.h"      
+  #define CONDITIONAL_LCD_START for (LCDDisplay * dummy=new LCDDisplay();dummy!=NULL; dummy=dummy->loop2(true))  
+
+#else 
+  #include "LCD_NONE.h"
+  #define CONDITIONAL_LCD_START if (true) /* NO LCD CONFIG, but do the LCD macros to get DIAGS */      
 #endif

 #endif // LCD_Implementation_h
--- a/LCN.h
+++ b/LCN.h
@@ -1,21 +1,3 @@
-/*
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 #ifndef LCN_h
 #define LCN_h
 #include <Arduino.h>
--- a/LiquidCrystal_I2C.cpp
+++ b/LiquidCrystal_I2C.cpp
@@ -20,7 +20,7 @@

 #include <Arduino.h>
 #include "LiquidCrystal_I2C.h"
-#include "DIAG.h"
+#include "I2CManager.h"

 // When the display powers up, it is configured as follows:
 //
@@ -44,30 +44,30 @@
 LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t lcd_cols,
                                     uint8_t lcd_rows) {
  _Addr = lcd_Addr;
-  lcdRows = lcd_rows;
-  lcdCols = lcd_cols;
+  _cols = lcd_cols;
+  _rows = lcd_rows;
+  _backlightval = LCD_NOBACKLIGHT;
+}

-  _backlightval = 0;
+void LiquidCrystal_I2C::init() { init_priv(); }

+void LiquidCrystal_I2C::init_priv() {
  I2CManager.begin();
  I2CManager.setClock(100000L);    // PCF8574 is spec'd to 100kHz.

-  if (I2CManager.exists(lcd_Addr)) {
-    DIAG(F("%dx%d LCD configured on I2C:x%x"), (int)lcd_cols, (int)lcd_rows, (int)lcd_Addr);
-    _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
-    begin();
-    backlight();
-    lcdDisplay = this;
-  }
+  _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
+  begin(_cols, _rows);
 }

-void LiquidCrystal_I2C::begin() {
-  if (lcdRows > 1) {
+void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines) {
+  if (lines > 1) {
    _displayfunction |= LCD_2LINE;
  }
+  _numlines = lines;
+  (void)cols; // Suppress compiler warning.

  // according to datasheet, we need at least 40ms after power rises above 2.7V
-  // before sending commands. Arduino can turn on way before 4.5V so we'll allow
+  // before sending commands. Arduino can turn on way befer 4.5V so we'll allow
  // 100 milliseconds after pulling both RS and R/W and backlight pin low
  expanderWrite(
      _backlightval);  // reset expander and turn backlight off (Bit 8 =1)
@@ -78,19 +78,19 @@ void LiquidCrystal_I2C::begin() {
  // figure 24, pg 46

  // we start in 8bit mode, try to set 4 bit mode
-  write4bits(0x03);
+  write4bits(0x03 << 4);
  delayMicroseconds(4500);  // wait min 4.1ms

  // second try
-  write4bits(0x03);
+  write4bits(0x03 << 4);
  delayMicroseconds(4500);  // wait min 4.1ms

  // third go!
-  write4bits(0x03);
+  write4bits(0x03 << 4);
  delayMicroseconds(150);

  // finally, set to 4-bit interface
-  write4bits(0x02);
+  write4bits(0x02 << 4);

  // set # lines, font size, etc.
  command(LCD_FUNCTIONSET | _displayfunction);
@@ -108,21 +108,27 @@ void LiquidCrystal_I2C::begin() {
  // set the entry mode
  command(LCD_ENTRYMODESET | _displaymode);

-  setRowNative(0);
+  setCursor(0, 0);
 }

 /********** high level commands, for the user! */
-void LiquidCrystal_I2C::clearNative() {
+void LiquidCrystal_I2C::clear() {
  command(LCD_CLEARDISPLAY);  // clear display, set cursor position to zero
  delayMicroseconds(2000);    // this command takes 1.52ms
 }

-void LiquidCrystal_I2C::setRowNative(byte row) {
+void LiquidCrystal_I2C::setCursor(uint8_t col, uint8_t row) {
  int row_offsets[] = {0x00, 0x40, 0x14, 0x54};
-  if (row > lcdRows) {
-    row = lcdRows - 1;  // we count rows starting w/0
+  if (row > _numlines) {
+    row = _numlines - 1;  // we count rows starting w/0
  }
-  command(LCD_SETDDRAMADDR | (row_offsets[row]));
+  command(LCD_SETDDRAMADDR | (col + row_offsets[row]));
+}
+
+// Turn the display on/off (quickly)
+void LiquidCrystal_I2C::noDisplay() {
+  _displaycontrol &= ~LCD_DISPLAYON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
 }

 void LiquidCrystal_I2C::display() {
@@ -132,7 +138,7 @@ void LiquidCrystal_I2C::display() {

 // Turn the (optional) backlight off/on
 void LiquidCrystal_I2C::noBacklight(void) {
-  _backlightval &= ~LCD_BACKLIGHT;
+  _backlightval = LCD_NOBACKLIGHT;
  expanderWrite(0);
 }

@@ -141,7 +147,7 @@ void LiquidCrystal_I2C::backlight(void) {
  expanderWrite(0);
 }

-size_t LiquidCrystal_I2C::writeNative(uint8_t value) {
+size_t LiquidCrystal_I2C::write(uint8_t value) {
  send(value, Rs);
  return 1;
 }
@@ -188,32 +194,25 @@ inline void LiquidCrystal_I2C::command(uint8_t value) {
 //  a single I2C transmission. 
 void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) {
  mode |= _backlightval;
-  uint8_t highnib = (((value >> 4) & 0x0f) << BACKPACK_DATA_BITS) | mode;
-  uint8_t lownib = ((value & 0x0f) << BACKPACK_DATA_BITS) | mode;
+  uint8_t highnib = (value & 0xf0) | mode;
+  uint8_t lownib = ((value << 4) & 0xf0) | mode;
  // Send both nibbles
-  uint8_t len = 0;
-  outputBuffer[len++] = highnib|En;
-  outputBuffer[len++] = highnib;
-  outputBuffer[len++] = lownib|En;
-  outputBuffer[len++] = lownib;
-  I2CManager.write(_Addr, outputBuffer, len);
+  byte buffer[] = {(byte)(highnib|En), highnib, (byte)(lownib|En), lownib};
+  I2CManager.write(_Addr, buffer, sizeof(buffer));
 }

-// write 4 data bits to the HD44780 LCD controller.
+// write 4 bits to the HD44780 LCD controller.
 void LiquidCrystal_I2C::write4bits(uint8_t value) {
-  uint8_t _data = ((value & 0x0f) << BACKPACK_DATA_BITS) | _backlightval;
+  uint8_t _data = value | _backlightval;
  // Enable must be set/reset for at least 450ns.  This is well within the
  // I2C clock cycle time of 2.5us at 400kHz. Data is clocked in to the
  // HD44780 on the trailing edge of the Enable pin.
-  uint8_t len = 0;
-  outputBuffer[len++] = _data|En;
-  outputBuffer[len++] = _data;
-  I2CManager.write(_Addr, outputBuffer, len);
+  byte buffer[] = {(byte)(_data|En), _data};
+  I2CManager.write(_Addr, buffer, sizeof(buffer));
 }

 // write a byte to the PCF8574 I2C interface.  We don't need to set
 // the enable pin for this.
 void LiquidCrystal_I2C::expanderWrite(uint8_t value) {
-  outputBuffer[0] = value | _backlightval;
-  I2CManager.write(_Addr, outputBuffer, 1);
+  I2CManager.write(_Addr, 1, value | _backlightval);
 }
--- a/LiquidCrystal_I2C.h
+++ b/LiquidCrystal_I2C.h
@@ -22,13 +22,13 @@
 #define LiquidCrystal_I2C_h

 #include <Arduino.h>
-#include "LCDDisplay.h"
-#include "I2CManager.h"

 // commands
 #define LCD_CLEARDISPLAY 0x01
+#define LCD_RETURNHOME 0x02
 #define LCD_ENTRYMODESET 0x04
 #define LCD_DISPLAYCONTROL 0x08
+#define LCD_CURSORSHIFT 0x10
 #define LCD_FUNCTIONSET 0x20
 #define LCD_SETCGRAMADDR 0x40
 #define LCD_SETDDRAMADDR 0x80
@@ -41,39 +41,46 @@

 // flags for display on/off control
 #define LCD_DISPLAYON 0x04
+#define LCD_DISPLAYOFF 0x00
+#define LCD_CURSORON 0x02
 #define LCD_CURSOROFF 0x00
+#define LCD_BLINKON 0x01
 #define LCD_BLINKOFF 0x00

+// flags for display/cursor shift
+#define LCD_DISPLAYMOVE 0x08
+#define LCD_CURSORMOVE 0x00
+#define LCD_MOVERIGHT 0x04
+#define LCD_MOVELEFT 0x00
+
 // flags for function set
+#define LCD_8BITMODE 0x10
 #define LCD_4BITMODE 0x00
 #define LCD_2LINE 0x08
 #define LCD_1LINE 0x00
+#define LCD_5x10DOTS 0x04
 #define LCD_5x8DOTS 0x00

-// Bit mapping onto PCF8574 port
-#define BACKPACK_Rs_BIT 0
-#define BACKPACK_Rw_BIT 1
-#define BACKPACK_En_BIT 2
-#define BACKPACK_BACKLIGHT_BIT 3
-#define BACKPACK_DATA_BITS 4 // Bits 4-7
-// Equivalent mask bits
-#define LCD_BACKLIGHT (1 << BACKPACK_BACKLIGHT_BIT)  // Backlight enable
-#define En (1 << BACKPACK_En_BIT)  // Enable bit
-#define Rw (1 << BACKPACK_Rw_BIT)  // Read/Write bit
-#define Rs (1 << BACKPACK_Rs_BIT)  // Register select bit
+// flags for backlight control
+#define LCD_BACKLIGHT 0x08
+#define LCD_NOBACKLIGHT 0x00

-class LiquidCrystal_I2C : public LCDDisplay {
+#define En 0b00000100  // Enable bit
+#define Rw 0b00000010  // Read/Write bit
+#define Rs 0b00000001  // Register select bit
+
+class LiquidCrystal_I2C : public Print {
 public:
  LiquidCrystal_I2C(uint8_t lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows);
-  void begin();
-  void clearNative();
-  void setRowNative(byte line);
-  size_t writeNative(uint8_t c);
-  
+  void begin(uint8_t cols, uint8_t rows);
+  void clear();
+  void noDisplay();
  void display();
  void noBacklight();
  void backlight();
  
+  void setCursor(uint8_t, uint8_t); 
+  virtual size_t write(uint8_t);
  void command(uint8_t);
  void init();

@@ -86,9 +93,10 @@ private:
  uint8_t _displayfunction;
  uint8_t _displaycontrol;
  uint8_t _displaymode;
+  uint8_t _numlines;
+  uint8_t _cols;
+  uint8_t _rows;
  uint8_t _backlightval;
-
-  uint8_t outputBuffer[4];
 };

 #endif
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -21,6 +21,11 @@
 #include "DCCTimer.h"
 #include "DIAG.h"

+#define setHIGH(fastpin)  *fastpin.inout |= fastpin.maskHIGH
+#define setLOW(fastpin)   *fastpin.inout &= fastpin.maskLOW
+#define isHIGH(fastpin)   (*fastpin.inout & fastpin.maskHIGH)
+#define isLOW(fastpin)    (!isHIGH(fastpin))
+
 bool MotorDriver::usePWM=false;
 bool MotorDriver::commonFaultPin=false;
       
@@ -51,6 +56,10 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8
    setBrake(false);
  }
  else brakePin=UNUSED_PIN;
+
+  // Initiate state of railcom cutout as off. This must be done
+  // independent of if railcom is used later or not.
+  setRailcomCutout(false);
  
  currentPin=current_pin;
  if (currentPin!=UNUSED_PIN) {
@@ -79,6 +88,10 @@ bool MotorDriver::isPWMCapable() {
    return (!dualSignal) && DCCTimer::isPWMPin(signalPin); 
 }

+bool MotorDriver::isRailcomCapable() {
+    return (!dualSignal) && DCCTimer::isRailcomPin(brakePin); 
+}
+

 void MotorDriver::setPower(bool on) {
  if (on) {
@@ -105,7 +118,12 @@ void MotorDriver::setBrake(bool on) {
  else setLOW(fastBrakePin);
 }

-void IRAM_ATTR MotorDriver::setSignal( bool high) {
+void MotorDriver::setRailcomCutout(bool on) {
+  DCCTimer::onoffPWM(signalPin,!on);
+  DCCTimer::setPWM(brakePin,on ^ invertBrake);
+}
+
+void MotorDriver::setSignal( bool high) {
   if (usePWM) {
    DCCTimer::setPWM(signalPin,high);
   }
@@ -171,8 +189,8 @@ int MotorDriver::mA2raw( unsigned int mA) {

 void  MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & result) {
    // DIAG(F("MotorDriver %S Pin=%d,"),type,pin);
-    (void) type; // avoid compiler warning if diag not used above.
-    PORTTYPE port = digitalPinToPort(pin);
+    (void) type; // avoid compiler warning if diag not used above. 
+    uint8_t port = digitalPinToPort(pin);
    if (input)
      result.inout = portInputRegister(port);
    else
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -26,15 +26,13 @@
 #define UNUSED_PIN 127 // inside int8_t
 #endif

-#if defined(__IMXRT1062__) || defined (ARDUINO_ARCH_ESP8266)
-typedef uint32_t PORTTYPE;
+#if defined(__IMXRT1062__)
 struct FASTPIN {
  volatile uint32_t *inout;
  uint32_t maskHIGH;  
  uint32_t maskLOW;  
 };
 #else
-typedef uint8_t PORTTYPE;
 struct FASTPIN {
  volatile uint8_t *inout;
  uint8_t maskHIGH;  
@@ -42,30 +40,12 @@ struct FASTPIN {
 };
 #endif

-#define setHIGH(fastpin)  *fastpin.inout |= fastpin.maskHIGH
-#define setLOW(fastpin)   *fastpin.inout &= fastpin.maskLOW
-#define isHIGH(fastpin)   (*fastpin.inout & fastpin.maskHIGH)
-#define isLOW(fastpin)    (!isHIGH(fastpin))
-
 class MotorDriver {
  public:
    MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin, 
                byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
    virtual void setPower( bool on);
-    void setSignal( bool high);/* {
-      if (usePWM) {
-	DCCTimer::setPWM(signalPin,high);
-      }
-      
-      if (high) {
-        setHIGH(fastSignalPin);
-        if (dualSignal) setLOW(fastSignalPin2);
-      }
-      else {
-        setLOW(fastSignalPin);
-        if (dualSignal) setHIGH(fastSignalPin2);
-      }
-      };*/
+    virtual void setSignal( bool high);
    virtual void setBrake( bool on);
    virtual int  getCurrentRaw();
    virtual unsigned int raw2mA( int raw);
@@ -74,7 +54,9 @@ class MotorDriver {
 	    return rawCurrentTripValue;
    }
    bool isPWMCapable();
+    bool isRailcomCapable();
    bool canMeasureCurrent();
+    void setRailcomCutout(bool on);
    static bool usePWM;
    static bool commonFaultPin; // This is a stupid motor shield which has only a common fault pin for both outputs
    inline byte getFaultPin() {
--- a/MotorDrivers.h
+++ b/MotorDrivers.h
@@ -1,24 +1,3 @@
-/*
- *  (c) 2020 Chris Harlow. All rights reserved.
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  (c) 2020 Harald Barth. All rights reserved.
- *  (c) 2020 Anthony W - Dayton. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 #ifndef MotorDrivers_h
 #define MotorDrivers_h
 #include <Arduino.h>
@@ -48,6 +27,11 @@
                              new MotorDriver(3, 12, UNUSED_PIN, UNUSED_PIN, A0, 2.99, 2000, UNUSED_PIN), \
                              new MotorDriver(11, 13, UNUSED_PIN, UNUSED_PIN, A1, 2.99, 2000, UNUSED_PIN)

+// Arduino standard Motor Shield with railcom (mega brakes on 6,7 require jumpers )
+#define STANDARD_WITH_RAILCOM F("STANDARD_WITH_RAILCOM"),                                                 \
+                              new MotorDriver(3, 12, UNUSED_PIN, 6, A0, 2.99, 2000, UNUSED_PIN), \
+                              new MotorDriver(11, 13, UNUSED_PIN, 7, A1, 2.99, 2000, UNUSED_PIN)
+
 // Pololu Motor Shield
 #define POLOLU_MOTOR_SHIELD F("POLOLU_MOTOR_SHIELD"),                                                 \
                            new MotorDriver( 9, 7, UNUSED_PIN,         -4, A0, 18, 3000, 12), \
@@ -63,6 +47,13 @@
 //                          new MotorDriver(2, 8, UNUSED_PIN, -10, A1, 18, 3000, 12)
 // See Pololu dial_mc33926_shield_schematic.pdf and truth table on page 17 of the MC33926 data sheet.

+// Pololu Dual TB9051FTG Motor Shield
+// This is the shield without modifications which means
+// no HA waveform and no RailCom on an Arduino Mega 2560
+#define POLOLU_TB9051FTG F("POLOLU_TB9051FTG"),                            \
+   new MotorDriver(2, 7, UNUSED_PIN,  -9, A0, 10, 2500,  6), \
+   new MotorDriver(4, 8, UNUSED_PIN, -10, A1, 10, 2500, 12)
+
 // Firebox Mk1
 #define FIREBOX_MK1 F("FIREBOX_MK1"),                                                  \
                    new MotorDriver(3, 6, 7, UNUSED_PIN, A5, 9.766, 5500, UNUSED_PIN), \
--- a/Outputs.cpp
+++ b/Outputs.cpp
@@ -100,14 +100,14 @@ void  Output::activate(int s){

 ///////////////////////////////////////////////////////////////////////////////

-Output* Output::get(uint16_t n){
+Output* Output::get(int n){
  Output *tt;
  for(tt=firstOutput;tt!=NULL && tt->data.id!=n;tt=tt->nextOutput);
  return(tt);
 }
 ///////////////////////////////////////////////////////////////////////////////

-bool Output::remove(uint16_t n){
+bool Output::remove(int n){
  Output *tt,*pp=NULL;

  for(tt=firstOutput;tt!=NULL && tt->data.id!=n;pp=tt,tt=tt->nextOutput);
@@ -127,46 +127,17 @@ bool Output::remove(uint16_t n){
 ///////////////////////////////////////////////////////////////////////////////

 void Output::load(){
-  struct BrokenOutputData bdata;
+  struct OutputData data;
  Output *tt;
-  bool isBroken=1;

-  // This is a scary kluge. As we have two formats in EEPROM due to an
-  // earlier bug, we don't know which we encounter now. So we guess
-  // that if in all entries this byte has value of 7 or lower this is
-  // an iFlag and thus the broken format. Otherwise it would be a pin
-  // id. If someone uses only pins 0 to 7 of their arduino, they
-  // loose. This is (if you look at an arduino) however unlikely.
-
-  for(uint16_t i=0;i<EEStore::eeStore->data.nOutputs;i++){
-    EEPROM.get(EEStore::pointer()+ i*sizeof(struct BrokenOutputData),bdata);
-    if (bdata.iFlag > 7) { // it's a pin and not an iFlag!
-      isBroken=0;
-      break;
-    }
-  }
-  if ( isBroken ) {
-    for(uint16_t i=0;i<EEStore::eeStore->data.nOutputs;i++){
-      EEPROM.get(EEStore::pointer(),bdata);
-      tt=create(bdata.id,bdata.pin,bdata.iFlag);
-      tt->data.oStatus=bitRead(tt->data.iFlag,1)?bitRead(tt->data.iFlag,2):bdata.oStatus;      // restore status to EEPROM value is bit 1 of iFlag=0, otherwise set to value of bit 2 of iFlag
-      digitalWrite(tt->data.pin,tt->data.oStatus ^ bitRead(tt->data.iFlag,0));
-      pinMode(tt->data.pin,OUTPUT);
-      tt->num=EEStore::pointer();
-      EEStore::advance(sizeof(struct BrokenOutputData));
-    }
-  } else {
-    struct OutputData data;
-
-    for(uint16_t i=0;i<EEStore::eeStore->data.nOutputs;i++){
-      EEPROM.get(EEStore::pointer(),data);
-      tt=create(data.id,data.pin,data.iFlag);
-      tt->data.oStatus=bitRead(tt->data.iFlag,1)?bitRead(tt->data.iFlag,2):data.oStatus;      // restore status to EEPROM value is bit 1 of iFlag=0, otherwise set to value of bit 2 of iFlag
-      digitalWrite(tt->data.pin,tt->data.oStatus ^ bitRead(tt->data.iFlag,0));
-      pinMode(tt->data.pin,OUTPUT);
-      tt->num=EEStore::pointer();
-      EEStore::advance(sizeof(struct OutputData));
-    }
+  for(int i=0;i<EEStore::eeStore->data.nOutputs;i++){
+    EEPROM.get(EEStore::pointer(),data);
+    tt=create(data.id,data.pin,data.iFlag);
+    tt->data.oStatus=bitRead(tt->data.iFlag,1)?bitRead(tt->data.iFlag,2):data.oStatus;      // restore status to EEPROM value is bit 1 of iFlag=0, otherwise set to value of bit 2 of iFlag
+    digitalWrite(tt->data.pin,tt->data.oStatus ^ bitRead(tt->data.iFlag,0));
+    pinMode(tt->data.pin,OUTPUT);
+    tt->num=EEStore::pointer();
+    EEStore::advance(sizeof(tt->data));
  }
 }

@@ -189,7 +160,7 @@ void Output::store(){
 }
 ///////////////////////////////////////////////////////////////////////////////

-Output *Output::create(uint16_t id, uint8_t pin, uint8_t iFlag, uint8_t v){
+Output *Output::create(int id, int pin, int iFlag, int v){
  Output *tt;

  if(firstOutput==NULL){
--- a/Outputs.h
+++ b/Outputs.h
@@ -23,34 +23,25 @@

 struct OutputData {
  uint8_t oStatus;
-  uint16_t id;
+  uint8_t id;
  uint8_t pin; 
  uint8_t iFlag; 
 };

-struct BrokenOutputData {
-  uint8_t oStatus;
-  uint8_t id;
-  uint8_t pin;
-  uint8_t iFlag;
-};
-
 class Output{
-
-public:
+  public:
  void activate(int s);
-  static Output* get(uint16_t);
-  static bool remove(uint16_t);
+  static Output* get(int);
+  static bool remove(int);
  static void load();
  static void store();
-  static Output *create(uint16_t, uint8_t, uint8_t, uint8_t=0);
+  static Output *create(int, int, int, int=0);
  static Output *firstOutput;
  struct OutputData data;
  Output *nextOutput;
  static void printAll(Print *);
-
-private:
-  int num;  // EEPROM pointer (Chris has no idea what this is all about!)
+  private:
+  int num;  // Chris has no idea what this is all about!
  
 }; // Output
  
--- a/PWMServoDriver.cpp
+++ b/PWMServoDriver.cpp
@@ -1,21 +1,5 @@
-/*
- *  (c) 2020 Chris Harlow. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
+
+
 /*!
 *  @file PWMServoDriver.cpp
 *
@@ -32,7 +16,11 @@
 *  
 *  @section author Author
 *  Chris Harlow (TPL) 
+ *  original by Limor Fried/Ladyada (Adafruit Industries).
 *
+ *  @section license License
+ *
+ *  BSD license, all text above must be included in any redistribution
 */
 #include <Arduino.h>
 #include "PWMServoDriver.h"
--- a/PWMServoDriver.h
+++ b/PWMServoDriver.h
@@ -1,21 +1,3 @@
-/*
- *  (c) 2020 Chris Harlow. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 /*!
 *  @file PWMServoDriver.h
 *
--- a/RingStream.cpp
+++ b/RingStream.cpp
@@ -45,11 +45,10 @@ size_t RingStream::write(uint8_t b) {
  return 1;
 }

-int RingStream::read(byte advance) {
-  if ((_pos_read==_pos_write) && !_overflow) return -1;  // empty
-  if (_pos_read == _mark) return -1;
+int RingStream::read() {
+  if ((_pos_read==_pos_write) && !_overflow) return -1;  // empty  
  byte b=_buffer[_pos_read];
-  _pos_read += advance;
+  _pos_read++;
  if (_pos_read==_len) _pos_read=0;
  _overflow=false;
  return b;
@@ -69,7 +68,6 @@ int RingStream::freeSpace() {

 // mark start of message with client id (0...9)
 void RingStream::mark(uint8_t b) {
-    //DIAG(F("Mark1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
    _mark=_pos_write;
    write(b); // client id
    write((uint8_t)0);  // count MSB placemarker
@@ -83,12 +81,7 @@ uint8_t RingStream::peekTargetMark() {
  return _buffer[_mark];
 }

-void RingStream::info() {
-  DIAG(F("Info len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
-}
-
 bool RingStream::commit() {
-  //DIAG(F("Commit1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
  if (_overflow) {
        DIAG(F("RingStream(%d) commit(%d) OVERFLOW"),_len, _count);
        // just throw it away 
@@ -108,7 +101,5 @@ bool RingStream::commit() {
  _mark++;
  if (_mark==_len) _mark=0;
  _buffer[_mark]=lowByte(_count);
-  _mark=_len+1;
-  //DIAG(F("Commit2 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
  return true; // commit worked
 }
--- a/RingStream.h
+++ b/RingStream.h
@@ -28,17 +28,14 @@ class RingStream : public Print {
  
    virtual size_t write(uint8_t b);
    using Print::write;
-    inline int read() { return read(1); };
-    inline int peek() { return read(0); };
+    int read();
    int count();
    int freeSpace();
    void mark(uint8_t b);
    bool commit();
    uint8_t peekTargetMark();
-    void info();
-
+    
 private:
-   int read(byte advance);
   int _len;
   int _pos_write;
   int _pos_read;
--- a/SSD1306Ascii.cpp
+++ b/SSD1306Ascii.cpp
@@ -1,8 +1,6 @@
 /* Based on Arduino SSD1306Ascii Library, Copyright (C) 2015 by William Greiman
 * Modifications (C) 2021 Neil McKechnie
 *
- *  This file is part of CommandStation-EX
- *
 * This Library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
@@ -21,375 +19,90 @@
 #include "I2CManager.h"
 #include "FSH.h"

-//==============================================================================
-// SSD1306/SSD1106 I2C command bytes
-//------------------------------------------------------------------------------
-/** Set Lower Column Start Address for Page Addressing Mode. */
-static const uint8_t SSD1306_SETLOWCOLUMN = 0x00;
-/** Set Higher Column Start Address for Page Addressing Mode. */
-static const uint8_t SSD1306_SETHIGHCOLUMN = 0x10;
-/** Set Memory Addressing Mode. */
-static const uint8_t SSD1306_MEMORYMODE = 0x20;
-/** Set display RAM display start line register from 0 - 63. */
-static const uint8_t SSD1306_SETSTARTLINE = 0x40;
-/** Set Display Contrast to one of 256 steps. */
-static const uint8_t SSD1306_SETCONTRAST = 0x81;
-/** Enable or disable charge pump.  Follow with 0X14 enable, 0X10 disable. */
-static const uint8_t SSD1306_CHARGEPUMP = 0x8D;
-/** Set Segment Re-map between data column and the segment driver. */
-static const uint8_t SSD1306_SEGREMAP = 0xA0;
-/** Resume display from GRAM content. */
-static const uint8_t SSD1306_DISPLAYALLON_RESUME = 0xA4;
-/** Force display on regardless of GRAM content. */
-static const uint8_t SSD1306_DISPLAYALLON = 0xA5;
-/** Set Normal Display. */
-static const uint8_t SSD1306_NORMALDISPLAY = 0xA6;
-/** Set Inverse Display. */
-static const uint8_t SSD1306_INVERTDISPLAY = 0xA7;
-/** Set Multiplex Ratio from 16 to 63. */
-static const uint8_t SSD1306_SETMULTIPLEX = 0xA8;
-/** Set Display off. */
-static const uint8_t SSD1306_DISPLAYOFF = 0xAE;
-/** Set Display on. */
-static const uint8_t SSD1306_DISPLAYON = 0xAF;
-/**Set GDDRAM Page Start Address. */
-static const uint8_t SSD1306_SETSTARTPAGE = 0xB0;
-/** Set COM output scan direction normal. */
-static const uint8_t SSD1306_COMSCANINC = 0xC0;
-/** Set COM output scan direction reversed. */
-static const uint8_t SSD1306_COMSCANDEC = 0xC8;
-/** Set Display Offset. */
-static const uint8_t SSD1306_SETDISPLAYOFFSET = 0xD3;
-/** Sets COM signals pin configuration to match the OLED panel layout. */
-static const uint8_t SSD1306_SETCOMPINS = 0xDA;
-/** This command adjusts the VCOMH regulator output. */
-static const uint8_t SSD1306_SETVCOMDETECT = 0xDB;
-/** Set Display Clock Divide Ratio/ Oscillator Frequency. */
-static const uint8_t SSD1306_SETDISPLAYCLOCKDIV = 0xD5;
-/** Set Pre-charge Period */
-static const uint8_t SSD1306_SETPRECHARGE = 0xD9;
-/** Deactivate scroll */
-static const uint8_t SSD1306_DEACTIVATE_SCROLL = 0x2E;
-/** No Operation Command. */
-static const uint8_t SSD1306_NOP = 0xE3;
-//------------------------------------------------------------------------------
-/** Set Pump voltage value: (30H~33H) 6.4, 7.4, 8.0 (POR), 9.0. */
-static const uint8_t SH1106_SET_PUMP_VOLTAGE = 0x30;
-/** First byte of set charge pump mode */
-static const uint8_t SH1106_SET_PUMP_MODE = 0xAD;
-/** Second byte charge pump on. */
-static const uint8_t SH1106_PUMP_ON = 0x8B;
-/** Second byte charge pump off. */
-static const uint8_t SH1106_PUMP_OFF = 0x8A;
-//------------------------------------------------------------------------------

-// Sequence of blank pixels, to optimise clearing screen.
-// Send a maximum of 30 pixels per transmission.
-const uint8_t FLASH SSD1306AsciiWire::blankPixels[30] = 
+// Maximum number of bytes we can send per transmission is 32.
+const uint8_t SSD1306AsciiWire::blankPixels[16] = 
  {0x40,        // First byte specifies data mode
-  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  
+  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  

 //==============================================================================
 // SSD1306AsciiWire Method Definitions
 //------------------------------------------------------------------------------
- 
-// Constructor
-SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
-  // Set size in characters in base class
-  lcdRows = height / 8;
-  lcdCols = width / 6;
-
-  I2CManager.begin();
-  I2CManager.setClock(400000L);  // Set max supported I2C speed
-  for (byte address = 0x3c; address <= 0x3d; address++) {
-    if (I2CManager.exists(address)) {
-      // Device found
-      DIAG(F("%dx%d OLED display configured on I2C:x%x"), width, height, address);
-      if (width == 132)
-        begin(&SH1106_132x64, address);
-      else if (height == 32)
-        begin(&Adafruit128x32, address);
-      else
-        begin(&Adafruit128x64, address);
-      // Set singleton address
-      lcdDisplay = this;
-      clear();
-      return;
-    }
-  }
-  DIAG(F("OLED display not found"));
+void SSD1306AsciiWire::clear() {
+  clear(0, displayWidth() - 1, 0, displayRows() - 1);
 }
-
-/* Clear screen by writing blank pixels. */
-void SSD1306AsciiWire::clearNative() {
+//------------------------------------------------------------------------------
+void SSD1306AsciiWire::clear(uint8_t columnStart, uint8_t columnEnd, 
+                             uint8_t rowStart, uint8_t rowEnd) {
  const int maxBytes = sizeof(blankPixels);  // max number of bytes sendable over Wire
-  for (uint8_t r = 0; r <= m_displayHeight/8 - 1; r++) {
-    setRowNative(r);   // Position at start of row to be erased
-    for (uint8_t c = 0; c <= m_displayWidth - 1; c += maxBytes-1) {
-      uint8_t len = min(m_displayWidth-c, maxBytes-1) + 1;
-      I2CManager.write_P(m_i2cAddr, blankPixels, len);  // Write a number of blank columns
+  // Ensure only rows on display will be cleared.
+  if (rowEnd >= displayRows()) rowEnd = displayRows() - 1;
+  for (uint8_t r = rowStart; r <= rowEnd; r++) {
+    setCursor(columnStart, r);   // Position at start of row to be erased
+    for (uint8_t c = columnStart; c <= columnEnd; c += maxBytes-1) {
+      uint8_t len = min((uint8_t)(columnEnd-c+1), maxBytes-1) + 1;
+      I2CManager.write(m_i2cAddr, blankPixels, len);  // Write up to 15 blank columns
    }
  }
 }
-
-// Initialise device
+//------------------------------------------------------------------------------
 void SSD1306AsciiWire::begin(const DevType* dev, uint8_t i2cAddr) {
  m_i2cAddr = i2cAddr;
  m_col = 0;
  m_row = 0;
-  const uint8_t* table = (const uint8_t*)GETFLASHW(&dev->initcmds);
-  uint8_t size = GETFLASH(&dev->initSize);
-  m_displayWidth = GETFLASH(&dev->lcdWidth);
-  m_displayHeight = GETFLASH(&dev->lcdHeight);
-  m_colOffset = GETFLASH(&dev->colOffset);
+#ifdef __AVR__
+  const uint8_t* table = (const uint8_t*)pgm_read_word(&dev->initcmds);
+#else   // __AVR__
+  const uint8_t* table = dev->initcmds;
+#endif  // __AVR
+  uint8_t size = readFontByte(&dev->initSize);
+  m_displayWidth = readFontByte(&dev->lcdWidth);
+  m_displayHeight = readFontByte(&dev->lcdHeight);
+  m_colOffset = readFontByte(&dev->colOffset);
  I2CManager.write_P(m_i2cAddr, table, size);
-  if (m_displayHeight == 32) 
-    I2CManager.write(m_i2cAddr, 5, 0, // Set command mode
-      SSD1306_SETMULTIPLEX, 0x1F,     // ratio 32
-      SSD1306_SETCOMPINS, 0x02);      // sequential COM pins, disable remap
 }
-
 //------------------------------------------------------------------------------
-
-// Set cursor position (by text line)
-void SSD1306AsciiWire::setRowNative(uint8_t line) {
-  // Calculate pixel position from line number
-  uint8_t row = line*8;
-  if (row < m_displayHeight) {
+void SSD1306AsciiWire::setContrast(uint8_t value) {
+  I2CManager.write(m_i2cAddr, 2, 
+    0x00,     // Set to command mode
+    SSD1306_SETCONTRAST, value);
+}
+//------------------------------------------------------------------------------
+void SSD1306AsciiWire::setCursor(uint8_t col, uint8_t row) {
+  if (row < displayRows() && col < m_displayWidth) {
    m_row = row;
-    m_col = m_colOffset;
-    // Build output buffer for I2C
-    uint8_t len = 0;
-    outputBuffer[len++] = 0x00;  // Set to command mode
-    outputBuffer[len++] = SSD1306_SETLOWCOLUMN | (m_col & 0XF);
-    outputBuffer[len++] = SSD1306_SETHIGHCOLUMN | (m_col >> 4);
-    outputBuffer[len++] = SSD1306_SETSTARTPAGE | (m_row/8);
-    I2CManager.write(m_i2cAddr, outputBuffer, len);
+    m_col = col + m_colOffset;
+    I2CManager.write(m_i2cAddr, 4,
+      0x00,    // Set to command mode
+      SSD1306_SETLOWCOLUMN | (col & 0XF), 
+      SSD1306_SETHIGHCOLUMN | (col >> 4),
+      SSD1306_SETSTARTPAGE | m_row);
  }
 }
 //------------------------------------------------------------------------------
-
-// Write a character to the OLED
-size_t SSD1306AsciiWire::writeNative(uint8_t ch) {
-  const uint8_t* base = m_font;
+void SSD1306AsciiWire::setFont(const uint8_t* font) {
+  m_font = font;
+  m_fontFirstChar = readFontByte(m_font + FONT_FIRST_CHAR);
+  m_fontCharCount = readFontByte(m_font + FONT_CHAR_COUNT);
+}
+//------------------------------------------------------------------------------
+size_t SSD1306AsciiWire::write(uint8_t ch) {
+  const uint8_t* base = m_font + FONT_WIDTH_TABLE;

  if (ch < m_fontFirstChar || ch >= (m_fontFirstChar + m_fontCharCount))
    return 0;
-  // Check if character would be partly or wholly off the display
-  if (m_col + fontWidth > m_displayWidth)
-    return 0;
-#if defined(NOLOWERCASE)
-  // Adjust if lowercase is missing
-  if (ch >= 'a') {
-    if (ch <= 'z')
-      ch = ch - 'a' + 'A';  // Capitalise
-    else
-      ch -= 26; // Allow for missing lowercase letters
-  }
-#endif
  ch -= m_fontFirstChar;
  base += fontWidth * ch;
-  // Build output buffer for I2C
-  outputBuffer[0] = 0x40;     // set SSD1306 controller to data mode
+  uint8_t buffer[1+fontWidth+letterSpacing];
+  buffer[0] = 0x40;     // set SSD1306 controller to data mode
  uint8_t bufferPos = 1;
  // Copy character pixel columns
  for (uint8_t i = 0; i < fontWidth; i++) 
-    outputBuffer[bufferPos++] = GETFLASH(base++);
+    buffer[bufferPos++] = readFontByte(base++);
  // Add blank pixels between letters
  for (uint8_t i = 0; i < letterSpacing; i++) 
-    outputBuffer[bufferPos++] = 0;
-
+    buffer[bufferPos++] = 0;
  // Write the data to I2C display
-  I2CManager.write(m_i2cAddr, outputBuffer, bufferPos);
-  m_col += fontWidth + letterSpacing;
+  I2CManager.write(m_i2cAddr, buffer, bufferPos);
  return 1;
 }
-
-//==============================================================================
-// this section is based on https://github.com/adafruit/Adafruit_SSD1306
-
-/** Initialization commands for a 128x32 or 128x64 SSD1306 oled display. */
-const uint8_t FLASH SSD1306AsciiWire::Adafruit128xXXinit[] = {
-    // Init sequence for Adafruit 128x32/64 OLED module
-    0x00,                              // Set to command mode
-    SSD1306_DISPLAYOFF,
-    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80 
-    SSD1306_SETMULTIPLEX, 0x3F,        // ratio 64 (initially)
-    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
-    SSD1306_SETSTARTLINE | 0x0,        // line #0
-    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
-    SSD1306_MEMORYMODE, 0x02,          // page mode
-    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
-    SSD1306_COMSCANDEC,                // column scan direction reversed
-    SSD1306_SETCOMPINS, 0X12,          // set COM pins
-    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
-    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
-    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
-    SSD1306_DISPLAYALLON_RESUME,
-    SSD1306_NORMALDISPLAY,
-    SSD1306_DISPLAYON
-};
-
-/** Initialize a 128x32 SSD1306 oled display. */
-const DevType FLASH SSD1306AsciiWire::Adafruit128x32 = {
-  Adafruit128xXXinit,
-  sizeof(Adafruit128xXXinit),
-  128,
-  32,
-  0
-};
-
-/** Initialize a 128x64 oled display. */
-const DevType FLASH SSD1306AsciiWire::Adafruit128x64 = {
-  Adafruit128xXXinit,
-  sizeof(Adafruit128xXXinit),
-  128,
-  64,
-  0
-};
-//------------------------------------------------------------------------------
-// This section is based on https://github.com/stanleyhuangyc/MultiLCD
-
-/** Initialization commands for a 128x64 SH1106 oled display. */
-const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
-  0x00,                                  // Set to command mode
-  SSD1306_DISPLAYOFF,
-  SSD1306_SETDISPLAYCLOCKDIV, 0X80,      // set osc division
-  SSD1306_SETMULTIPLEX, 0x3F,            // ratio 64
-  SSD1306_SETDISPLAYOFFSET, 0X00,        // set display offset
-  SSD1306_SETSTARTPAGE | 0X0,            // set page address
-  SSD1306_SETSTARTLINE | 0x0,            // set start line
-  SH1106_SET_PUMP_MODE, SH1106_PUMP_ON,  // set charge pump enable
-  SSD1306_SEGREMAP | 0X1,                // set segment remap
-  SSD1306_COMSCANDEC,                    // Com scan direction
-  SSD1306_SETCOMPINS, 0X12,              // set COM pins
-  SSD1306_SETCONTRAST, 0x80,             // 128
-  SSD1306_SETPRECHARGE, 0X1F,            // set pre-charge period
-  SSD1306_SETVCOMDETECT,  0x40,          // set vcomh
-  SH1106_SET_PUMP_VOLTAGE | 0X2,         // 8.0 volts
-  SSD1306_NORMALDISPLAY,                 // normal / reverse
-  SSD1306_DISPLAYON
-};
-
-/** Initialize a 132x64 oled SH1106 display. */
-const DevType FLASH SSD1306AsciiWire::SH1106_132x64 =  {
-  SH1106_132x64init,
-  sizeof(SH1106_132x64init),
-  128,
-  64,
-  2    // SH1106 is a 132x64 controller but most OLEDs are only attached
-       // to columns 2-129.
-};
-
-
-//------------------------------------------------------------------------------
-
-// Font characters, 5x7 pixels, 0x61 characters starting at 0x20.
-// Lower case characters optionally omitted.
-const uint8_t FLASH SSD1306AsciiWire::System5x7[] = {
-
-    // Fixed width; char width table not used !!!!
-    // or with lowercase character omitted.
-
-    // font data
-    0x00, 0x00, 0x00, 0x00, 0x00,  // (space)
-    0x00, 0x00, 0x5F, 0x00, 0x00,  // !
-    0x00, 0x07, 0x00, 0x07, 0x00,  // "
-    0x14, 0x7F, 0x14, 0x7F, 0x14,  // #
-    0x24, 0x2A, 0x7F, 0x2A, 0x12,  // $
-    0x23, 0x13, 0x08, 0x64, 0x62,  // %
-    0x36, 0x49, 0x55, 0x22, 0x50,  // &
-    0x00, 0x05, 0x03, 0x00, 0x00,  // '
-    0x00, 0x1C, 0x22, 0x41, 0x00,  // (
-    0x00, 0x41, 0x22, 0x1C, 0x00,  // )
-    0x08, 0x2A, 0x1C, 0x2A, 0x08,  // *
-    0x08, 0x08, 0x3E, 0x08, 0x08,  // +
-    0x00, 0x50, 0x30, 0x00, 0x00,  // ,
-    0x08, 0x08, 0x08, 0x08, 0x08,  // -
-    0x00, 0x60, 0x60, 0x00, 0x00,  // .
-    0x20, 0x10, 0x08, 0x04, 0x02,  // /
-    0x3E, 0x51, 0x49, 0x45, 0x3E,  // 0
-    0x00, 0x42, 0x7F, 0x40, 0x00,  // 1
-    0x42, 0x61, 0x51, 0x49, 0x46,  // 2
-    0x21, 0x41, 0x45, 0x4B, 0x31,  // 3
-    0x18, 0x14, 0x12, 0x7F, 0x10,  // 4
-    0x27, 0x45, 0x45, 0x45, 0x39,  // 5
-    0x3C, 0x4A, 0x49, 0x49, 0x30,  // 6
-    0x01, 0x71, 0x09, 0x05, 0x03,  // 7
-    0x36, 0x49, 0x49, 0x49, 0x36,  // 8
-    0x06, 0x49, 0x49, 0x29, 0x1E,  // 9
-    0x00, 0x36, 0x36, 0x00, 0x00,  // :
-    0x00, 0x56, 0x36, 0x00, 0x00,  // ;
-    0x00, 0x08, 0x14, 0x22, 0x41,  // <
-    0x14, 0x14, 0x14, 0x14, 0x14,  // =
-    0x41, 0x22, 0x14, 0x08, 0x00,  // >
-    0x02, 0x01, 0x51, 0x09, 0x06,  // ?
-    0x32, 0x49, 0x79, 0x41, 0x3E,  // @
-    0x7E, 0x11, 0x11, 0x11, 0x7E,  // A
-    0x7F, 0x49, 0x49, 0x49, 0x36,  // B
-    0x3E, 0x41, 0x41, 0x41, 0x22,  // C
-    0x7F, 0x41, 0x41, 0x22, 0x1C,  // D
-    0x7F, 0x49, 0x49, 0x49, 0x41,  // E
-    0x7F, 0x09, 0x09, 0x01, 0x01,  // F
-    0x3E, 0x41, 0x41, 0x51, 0x32,  // G
-    0x7F, 0x08, 0x08, 0x08, 0x7F,  // H
-    0x00, 0x41, 0x7F, 0x41, 0x00,  // I
-    0x20, 0x40, 0x41, 0x3F, 0x01,  // J
-    0x7F, 0x08, 0x14, 0x22, 0x41,  // K
-    0x7F, 0x40, 0x40, 0x40, 0x40,  // L
-    0x7F, 0x02, 0x04, 0x02, 0x7F,  // M
-    0x7F, 0x04, 0x08, 0x10, 0x7F,  // N
-    0x3E, 0x41, 0x41, 0x41, 0x3E,  // O
-    0x7F, 0x09, 0x09, 0x09, 0x06,  // P
-    0x3E, 0x41, 0x51, 0x21, 0x5E,  // Q
-    0x7F, 0x09, 0x19, 0x29, 0x46,  // R
-    0x46, 0x49, 0x49, 0x49, 0x31,  // S
-    0x01, 0x01, 0x7F, 0x01, 0x01,  // T
-    0x3F, 0x40, 0x40, 0x40, 0x3F,  // U
-    0x1F, 0x20, 0x40, 0x20, 0x1F,  // V
-    0x7F, 0x20, 0x18, 0x20, 0x7F,  // W
-    0x63, 0x14, 0x08, 0x14, 0x63,  // X
-    0x03, 0x04, 0x78, 0x04, 0x03,  // Y
-    0x61, 0x51, 0x49, 0x45, 0x43,  // Z
-    0x00, 0x00, 0x7F, 0x41, 0x41,  // [
-    0x02, 0x04, 0x08, 0x10, 0x20,  // "\"
-    0x41, 0x41, 0x7F, 0x00, 0x00,  // ]
-    0x04, 0x02, 0x01, 0x02, 0x04,  // ^
-    0x40, 0x40, 0x40, 0x40, 0x40,  // _
-    0x00, 0x01, 0x02, 0x04, 0x00,  // `
-#ifndef NOLOWERCASE
-    0x20, 0x54, 0x54, 0x54, 0x78,  // a
-    0x7F, 0x48, 0x44, 0x44, 0x38,  // b
-    0x38, 0x44, 0x44, 0x44, 0x20,  // c
-    0x38, 0x44, 0x44, 0x48, 0x7F,  // d
-    0x38, 0x54, 0x54, 0x54, 0x18,  // e
-    0x08, 0x7E, 0x09, 0x01, 0x02,  // f
-    0x08, 0x14, 0x54, 0x54, 0x3C,  // g
-    0x7F, 0x08, 0x04, 0x04, 0x78,  // h
-    0x00, 0x44, 0x7D, 0x40, 0x00,  // i
-    0x20, 0x40, 0x44, 0x3D, 0x00,  // j
-    0x00, 0x7F, 0x10, 0x28, 0x44,  // k
-    0x00, 0x41, 0x7F, 0x40, 0x00,  // l
-    0x7C, 0x04, 0x18, 0x04, 0x78,  // m
-    0x7C, 0x08, 0x04, 0x04, 0x78,  // n
-    0x38, 0x44, 0x44, 0x44, 0x38,  // o
-    0x7C, 0x14, 0x14, 0x14, 0x08,  // p
-    0x08, 0x14, 0x14, 0x18, 0x7C,  // q
-    0x7C, 0x08, 0x04, 0x04, 0x08,  // r
-    0x48, 0x54, 0x54, 0x54, 0x20,  // s
-    0x04, 0x3F, 0x44, 0x40, 0x20,  // t
-    0x3C, 0x40, 0x40, 0x20, 0x7C,  // u
-    0x1C, 0x20, 0x40, 0x20, 0x1C,  // v
-    0x3C, 0x40, 0x30, 0x40, 0x3C,  // w
-    0x44, 0x28, 0x10, 0x28, 0x44,  // x
-    0x0C, 0x50, 0x50, 0x50, 0x3C,  // y
-    0x44, 0x64, 0x54, 0x4C, 0x44,  // z
-#endif
-    0x00, 0x08, 0x36, 0x41, 0x00,  // {
-    0x00, 0x00, 0x7F, 0x00, 0x00,  // |
-    0x00, 0x41, 0x36, 0x08, 0x00,  // }
-    0x08, 0x08, 0x2A, 0x1C, 0x08,  // ->
-    0x08, 0x1C, 0x2A, 0x08, 0x08,  // <-
-    0x00, 0x06, 0x09, 0x09, 0x06   // degree symbol
-
-};
--- a/SSD1306Ascii.h
+++ b/SSD1306Ascii.h
@@ -1,8 +1,6 @@
 /* Based on Arduino SSD1306Ascii Library, Copyright (C) 2015 by William Greiman
 * Modifications (C) 2021 Neil McKechnie
 *
- *  This file is part of CommandStation-EX
- *
 * This Library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
@@ -22,88 +20,78 @@
 #define SSD1306Ascii_h

 #include "Arduino.h"
-#include "FSH.h"
-#include "LCDDisplay.h"
+#include "SSD1306font.h"
+#include "SSD1306init.h"

-#include "I2CManager.h"
-#include "DIAG.h"
-
-// Uncomment to remove lower-case letters to save 108 bytes of flash
-//#define NOLOWERCASE
-
-//------------------------------------------------------------------------------
-// Device initialization structure.
-
-struct DevType {
-  /* Pointer to initialization command bytes. */
-  const uint8_t* initcmds;
-  /* Number of initialization bytes */
-  const uint8_t initSize;
-  /* Width of the display in pixels */
-  const uint8_t lcdWidth;
-  /** Height of the display in pixels. */
-  const uint8_t lcdHeight;
-  /* Column offset RAM to display.  Used to pick start column of SH1106. */
-  const uint8_t colOffset;
-};
-
-// Constructor
-class SSD1306AsciiWire : public LCDDisplay {
+class SSD1306AsciiWire : public Print {
 public:
-
-  // Constructor
-  SSD1306AsciiWire(int width, int height);
-
+  using Print::write;
+  SSD1306AsciiWire() {}
  // Initialize the display controller.
  void begin(const DevType* dev, uint8_t i2cAddr);
-
  // Clear the display and set the cursor to (0, 0).
-  void clearNative();
-
-  // Set cursor to start of specified text line
-  void setRowNative(byte line);
-
+  void clear();
+  // Clear a region of the display.
+  void clear(uint8_t c0, uint8_t c1, uint8_t r0, uint8_t r1);
+  // The current column in pixels.
+  inline uint8_t col() const { return m_col; }
+  // The display hight in pixels.
+  inline uint8_t displayHeight() const { return m_displayHeight; }
+  // The display height in rows with eight pixels to a row.
+  inline uint8_t displayRows() const { return m_displayHeight / 8; }
+  // The display width in pixels.
+  inline uint8_t displayWidth() const { return m_displayWidth; }
+  // Set the cursor position to (0, 0).
+  inline void home() { setCursor(0, 0); }
  // Initialize the display controller.
  void init(const DevType* dev);
-  
-  // Write one character to OLED
-  size_t writeNative(uint8_t c);
-
-  // Display characteristics / initialisation
-  static const DevType FLASH Adafruit128x32;
-  static const DevType FLASH Adafruit128x64;
-  static const DevType FLASH SH1106_132x64;
+  // the current row number with eight pixels to a row.
+  inline uint8_t row() const { return m_row; }
+  /**
+   * @brief Set the display contrast.
+   *
+   * @param[in] value The contrast level in th range 0 to 255.
+   */
+  void setContrast(uint8_t value);
+  /**
+   * @brief Set the cursor position.
+   *
+   * @param[in] col The column number in pixels.
+   * @param[in] row the row number in eight pixel rows.
+   */
+  void setCursor(uint8_t col, uint8_t row);
+  /**
+   * @brief Set the current font.
+   *
+   * @param[in] font Pointer to a font table.
+   */
+  void setFont(const uint8_t* font);
+  /**
+   * @brief Display a character.
+   *
+   * @param[in] c The character to display.
+   * @return one for success else zero.
+   */
+  size_t write(uint8_t c);

 private:
-  // Cursor column.
-  uint8_t m_col;
-  // Cursor RAM row.
-  uint8_t m_row;
-  // Display width.
-  uint8_t m_displayWidth;
-  // Display height.
-  uint8_t m_displayHeight;
-  // Column offset RAM to SEG.
-  uint8_t m_colOffset = 0;
-  // Current font.
-  const uint8_t* const m_font = System5x7;
+  uint8_t m_col;                    // Cursor column.
+  uint8_t m_row;                    // Cursor RAM row.
+  uint8_t m_displayWidth;           // Display width.
+  uint8_t m_displayHeight;          // Display height.
+  uint8_t m_colOffset;              // Column offset RAM to SEG.
+  const uint8_t* m_font = NULL;     // Current font.

  // Only fixed size 5x7 fonts in a 6x8 cell are supported.
-  static const uint8_t fontWidth = 5;
-  static const uint8_t fontHeight = 7;
-  static const uint8_t letterSpacing = 1;
-  static const uint8_t m_fontFirstChar = 0x20;
-  static const uint8_t m_fontCharCount = 0x61;
+  const uint8_t fontWidth = 5;
+  const uint8_t fontHeight = 7;
+  const uint8_t letterSpacing = 1;
+  uint8_t m_fontFirstChar;
+  uint8_t m_fontCharCount;

  uint8_t m_i2cAddr;

-  uint8_t outputBuffer[fontWidth+letterSpacing+1];
-
  static const uint8_t blankPixels[];
-
-  static const uint8_t System5x7[];
-  static const uint8_t FLASH Adafruit128xXXinit[];
-  static const uint8_t FLASH SH1106_132x64init[];
 };

 #endif  // SSD1306Ascii_h
--- a/SSD1306font.h
+++ b/SSD1306font.h
@@ -0,0 +1,180 @@
+/*
+ *
+ * System5x7
+ *
+ *
+ * File Name           : System5x7.h
+ * Date                : 28 Oct 2008
+ * Font size in bytes  : 470
+ * Font width          : 5
+ * Font height         : 7
+ * Font first char     : 32
+ * Font last char      : 127
+ * Font used chars     : 94
+ *
+ * The font data are defined as
+ *
+ * struct _FONT_ {
+ *     uint16_t   font_Size_in_Bytes_over_all_included_Size_it_self;
+ *     uint8_t    font_Width_in_Pixel_for_fixed_drawing;
+ *     uint8_t    font_Height_in_Pixel_for_all_characters;
+ *     unit8_t    font_First_Char;
+ *     uint8_t    font_Char_Count;
+ *
+ *     uint8_t    font_Char_Widths[font_Last_Char - font_First_Char +1];
+ *                  // for each character the separate width in pixels,
+ *                  // characters < 128 have an implicit virtual right empty row
+ *
+ *     uint8_t    font_data[];
+ *                  // bit field of all characters
+ */
+
+#ifndef SSD1306font_H
+#define SSD1306font_H
+
+#define SYSTEM5x7_WIDTH 5
+#define SYSTEM5x7_HEIGHT 7
+
+#ifdef __AVR__
+#include <avr/pgmspace.h>
+/** declare a font for AVR. */
+#define GLCDFONTDECL(_n) static const uint8_t __attribute__((progmem)) _n[]
+#define readFontByte(addr) pgm_read_byte(addr)
+#else  // __AVR__
+/** declare a font. */
+#define GLCDFONTDECL(_n) static const uint8_t _n[]
+/** Fake read from flash. */
+#define readFontByte(addr) (*(const unsigned char *)(addr))
+#endif  // __AVR__
+//------------------------------------------------------------------------------
+// Font Indices
+/** No longer used Big Endian length field. Now indicates font type.
+ *
+ * 00 00 (fixed width font with 1 padding pixel on right and below)
+ *
+ * 00 01 (fixed width font with no padding pixels)
+ */
+#define FONT_LENGTH 0
+/** Maximum character width. */
+#define FONT_WIDTH 2
+/** Font hight in pixels */
+#define FONT_HEIGHT 3
+/** Ascii value of first character */
+#define FONT_FIRST_CHAR 4
+/** count of characters in font. */
+#define FONT_CHAR_COUNT 5
+/** Offset to width table. */
+#define FONT_WIDTH_TABLE 6
+//------------------------------------------------------------------------------
+
+GLCDFONTDECL(System5x7) = {
+    0x0, 0x0,  // size of zero indicates fixed width font,
+    0x05,      // width
+    0x07,      // height
+    0x20,      // first char
+    0x61,      // char count
+
+    // Fixed width; char width table not used !!!!
+
+    // font data
+    0x00, 0x00, 0x00, 0x00, 0x00,  // (space)
+    0x00, 0x00, 0x5F, 0x00, 0x00,  // !
+    0x00, 0x07, 0x00, 0x07, 0x00,  // "
+    0x14, 0x7F, 0x14, 0x7F, 0x14,  // #
+    0x24, 0x2A, 0x7F, 0x2A, 0x12,  // $
+    0x23, 0x13, 0x08, 0x64, 0x62,  // %
+    0x36, 0x49, 0x55, 0x22, 0x50,  // &
+    0x00, 0x05, 0x03, 0x00, 0x00,  // '
+    0x00, 0x1C, 0x22, 0x41, 0x00,  // (
+    0x00, 0x41, 0x22, 0x1C, 0x00,  // )
+    0x08, 0x2A, 0x1C, 0x2A, 0x08,  // *
+    0x08, 0x08, 0x3E, 0x08, 0x08,  // +
+    0x00, 0x50, 0x30, 0x00, 0x00,  // ,
+    0x08, 0x08, 0x08, 0x08, 0x08,  // -
+    0x00, 0x60, 0x60, 0x00, 0x00,  // .
+    0x20, 0x10, 0x08, 0x04, 0x02,  // /
+    0x3E, 0x51, 0x49, 0x45, 0x3E,  // 0
+    0x00, 0x42, 0x7F, 0x40, 0x00,  // 1
+    0x42, 0x61, 0x51, 0x49, 0x46,  // 2
+    0x21, 0x41, 0x45, 0x4B, 0x31,  // 3
+    0x18, 0x14, 0x12, 0x7F, 0x10,  // 4
+    0x27, 0x45, 0x45, 0x45, 0x39,  // 5
+    0x3C, 0x4A, 0x49, 0x49, 0x30,  // 6
+    0x01, 0x71, 0x09, 0x05, 0x03,  // 7
+    0x36, 0x49, 0x49, 0x49, 0x36,  // 8
+    0x06, 0x49, 0x49, 0x29, 0x1E,  // 9
+    0x00, 0x36, 0x36, 0x00, 0x00,  // :
+    0x00, 0x56, 0x36, 0x00, 0x00,  // ;
+    0x00, 0x08, 0x14, 0x22, 0x41,  // <
+    0x14, 0x14, 0x14, 0x14, 0x14,  // =
+    0x41, 0x22, 0x14, 0x08, 0x00,  // >
+    0x02, 0x01, 0x51, 0x09, 0x06,  // ?
+    0x32, 0x49, 0x79, 0x41, 0x3E,  // @
+    0x7E, 0x11, 0x11, 0x11, 0x7E,  // A
+    0x7F, 0x49, 0x49, 0x49, 0x36,  // B
+    0x3E, 0x41, 0x41, 0x41, 0x22,  // C
+    0x7F, 0x41, 0x41, 0x22, 0x1C,  // D
+    0x7F, 0x49, 0x49, 0x49, 0x41,  // E
+    0x7F, 0x09, 0x09, 0x01, 0x01,  // F
+    0x3E, 0x41, 0x41, 0x51, 0x32,  // G
+    0x7F, 0x08, 0x08, 0x08, 0x7F,  // H
+    0x00, 0x41, 0x7F, 0x41, 0x00,  // I
+    0x20, 0x40, 0x41, 0x3F, 0x01,  // J
+    0x7F, 0x08, 0x14, 0x22, 0x41,  // K
+    0x7F, 0x40, 0x40, 0x40, 0x40,  // L
+    0x7F, 0x02, 0x04, 0x02, 0x7F,  // M
+    0x7F, 0x04, 0x08, 0x10, 0x7F,  // N
+    0x3E, 0x41, 0x41, 0x41, 0x3E,  // O
+    0x7F, 0x09, 0x09, 0x09, 0x06,  // P
+    0x3E, 0x41, 0x51, 0x21, 0x5E,  // Q
+    0x7F, 0x09, 0x19, 0x29, 0x46,  // R
+    0x46, 0x49, 0x49, 0x49, 0x31,  // S
+    0x01, 0x01, 0x7F, 0x01, 0x01,  // T
+    0x3F, 0x40, 0x40, 0x40, 0x3F,  // U
+    0x1F, 0x20, 0x40, 0x20, 0x1F,  // V
+    0x7F, 0x20, 0x18, 0x20, 0x7F,  // W
+    0x63, 0x14, 0x08, 0x14, 0x63,  // X
+    0x03, 0x04, 0x78, 0x04, 0x03,  // Y
+    0x61, 0x51, 0x49, 0x45, 0x43,  // Z
+    0x00, 0x00, 0x7F, 0x41, 0x41,  // [
+    0x02, 0x04, 0x08, 0x10, 0x20,  // "\"
+    0x41, 0x41, 0x7F, 0x00, 0x00,  // ]
+    0x04, 0x02, 0x01, 0x02, 0x04,  // ^
+    0x40, 0x40, 0x40, 0x40, 0x40,  // _
+    0x00, 0x01, 0x02, 0x04, 0x00,  // `
+    0x20, 0x54, 0x54, 0x54, 0x78,  // a
+    0x7F, 0x48, 0x44, 0x44, 0x38,  // b
+    0x38, 0x44, 0x44, 0x44, 0x20,  // c
+    0x38, 0x44, 0x44, 0x48, 0x7F,  // d
+    0x38, 0x54, 0x54, 0x54, 0x18,  // e
+    0x08, 0x7E, 0x09, 0x01, 0x02,  // f
+    0x08, 0x14, 0x54, 0x54, 0x3C,  // g
+    0x7F, 0x08, 0x04, 0x04, 0x78,  // h
+    0x00, 0x44, 0x7D, 0x40, 0x00,  // i
+    0x20, 0x40, 0x44, 0x3D, 0x00,  // j
+    0x00, 0x7F, 0x10, 0x28, 0x44,  // k
+    0x00, 0x41, 0x7F, 0x40, 0x00,  // l
+    0x7C, 0x04, 0x18, 0x04, 0x78,  // m
+    0x7C, 0x08, 0x04, 0x04, 0x78,  // n
+    0x38, 0x44, 0x44, 0x44, 0x38,  // o
+    0x7C, 0x14, 0x14, 0x14, 0x08,  // p
+    0x08, 0x14, 0x14, 0x18, 0x7C,  // q
+    0x7C, 0x08, 0x04, 0x04, 0x08,  // r
+    0x48, 0x54, 0x54, 0x54, 0x20,  // s
+    0x04, 0x3F, 0x44, 0x40, 0x20,  // t
+    0x3C, 0x40, 0x40, 0x20, 0x7C,  // u
+    0x1C, 0x20, 0x40, 0x20, 0x1C,  // v
+    0x3C, 0x40, 0x30, 0x40, 0x3C,  // w
+    0x44, 0x28, 0x10, 0x28, 0x44,  // x
+    0x0C, 0x50, 0x50, 0x50, 0x3C,  // y
+    0x44, 0x64, 0x54, 0x4C, 0x44,  // z
+    0x00, 0x08, 0x36, 0x41, 0x00,  // {
+    0x00, 0x00, 0x7F, 0x00, 0x00,  // |
+    0x00, 0x41, 0x36, 0x08, 0x00,  // }
+    0x08, 0x08, 0x2A, 0x1C, 0x08,  // ->
+    0x08, 0x1C, 0x2A, 0x08, 0x08,  // <-
+    0x00, 0x06, 0x09, 0x09, 0x06   // degree symbol
+
+};
+
+#endif
--- a/SSD1306init.h
+++ b/SSD1306init.h
@@ -0,0 +1,207 @@
+/* Based on Arduino SSD1306Ascii Library, Copyright (C) 2015 by William Greiman
+ * Modifications (C) 2021 Neil McKechnie
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SSD1306Ascii Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file SSD1306init.h
+ * @brief Display controller initialization commands.
+ */
+#ifndef SSD1306init_h
+#define SSD1306init_h
+//------------------------------------------------------------------------------
+#ifndef __AVR__
+/** Handle AVR flash addressing. */
+#define MEM_TYPE
+#else  // __AVR__
+#define MEM_TYPE __attribute__ ((progmem))
+#endif  // __AVR__
+//------------------------------------------------------------------------------
+/** Set Lower Column Start Address for Page Addressing Mode. */
+#define SSD1306_SETLOWCOLUMN 0x00
+/** Set Higher Column Start Address for Page Addressing Mode. */
+#define SSD1306_SETHIGHCOLUMN 0x10
+/** Set Memory Addressing Mode. */
+#define SSD1306_MEMORYMODE 0x20
+/** Set display RAM display start line register from 0 - 63. */
+#define SSD1306_SETSTARTLINE 0x40
+/** Set Display Contrast to one of 256 steps. */
+#define SSD1306_SETCONTRAST 0x81
+/** Enable or disable charge pump.  Follow with 0X14 enable, 0X10 disable. */
+#define SSD1306_CHARGEPUMP 0x8D
+/** Set Segment Re-map between data column and the segment driver. */
+#define SSD1306_SEGREMAP 0xA0
+/** Resume display from GRAM content. */
+#define SSD1306_DISPLAYALLON_RESUME 0xA4
+/** Force display on regardless of GRAM content. */
+#define SSD1306_DISPLAYALLON 0xA5
+/** Set Normal Display. */
+#define SSD1306_NORMALDISPLAY 0xA6
+/** Set Inverse Display. */
+#define SSD1306_INVERTDISPLAY 0xA7
+/** Set Multiplex Ratio from 16 to 63. */
+#define SSD1306_SETMULTIPLEX 0xA8
+/** Set Display off. */
+#define SSD1306_DISPLAYOFF 0xAE
+/** Set Display on. */
+#define SSD1306_DISPLAYON 0xAF
+/**Set GDDRAM Page Start Address. */
+#define SSD1306_SETSTARTPAGE 0XB0
+/** Set COM output scan direction normal. */
+#define SSD1306_COMSCANINC 0xC0
+/** Set COM output scan direction reversed. */
+#define SSD1306_COMSCANDEC 0xC8
+/** Set Display Offset. */
+#define SSD1306_SETDISPLAYOFFSET 0xD3
+/** Sets COM signals pin configuration to match the OLED panel layout. */
+#define SSD1306_SETCOMPINS 0xDA
+/** This command adjusts the VCOMH regulator output. */
+#define SSD1306_SETVCOMDETECT 0xDB
+/** Set Display Clock Divide Ratio/ Oscillator Frequency. */
+#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
+/** Set Pre-charge Period */
+#define SSD1306_SETPRECHARGE 0xD9
+/** Deactivate scroll */
+#define SSD1306_DEACTIVATE_SCROLL 0x2E
+/** No Operation Command. */
+#define SSD1306_NOP 0XE3
+//------------------------------------------------------------------------------
+/** Set Pump voltage value: (30H~33H) 6.4, 7.4, 8.0 (POR), 9.0. */
+#define SH1106_SET_PUMP_VOLTAGE 0X30
+/** First byte of set charge pump mode */
+#define SH1106_SET_PUMP_MODE 0XAD
+/** Second byte charge pump on. */
+#define SH1106_PUMP_ON 0X8B
+/** Second byte charge pump off. */
+#define SH1106_PUMP_OFF 0X8A
+//------------------------------------------------------------------------------
+/**
+ * @struct DevType
+ * @brief Device initialization structure.
+ */
+struct DevType {
+  /**
+   * Pointer to initialization command bytes.
+   */
+  const uint8_t* initcmds;
+  /**
+   * Number of initialization bytes.
+   */
+  const uint8_t initSize;
+  /**
+   * Width of the diaplay in pixels.
+   */
+  const uint8_t lcdWidth;
+  /**
+   * Height of the display in pixels.
+   */
+  const uint8_t lcdHeight;
+  /**
+   * Column offset RAM to display.  Used to pick start column of SH1106.
+   */
+  const uint8_t colOffset;
+};
+//------------------------------------------------------------------------------
+// this section is based on https://github.com/adafruit/Adafruit_SSD1306
+/** Initialization commands for a 128x32 SSD1306 oled display. */
+static const uint8_t MEM_TYPE Adafruit128x32init[] = {
+    // Init sequence for Adafruit 128x32 OLED module
+    0x00,                              // Set to command mode
+    SSD1306_DISPLAYOFF,
+    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80
+    SSD1306_SETMULTIPLEX, 0x1F,        // ratio 32
+    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
+    SSD1306_SETSTARTLINE | 0x0,        // line #0
+    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
+    SSD1306_MEMORYMODE, 0x02,          // page mode
+    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
+    SSD1306_COMSCANDEC,                // column scan direction reversed
+    SSD1306_SETCOMPINS, 0x02,          // sequential COM pins, disable remap
+    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
+    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
+    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
+    SSD1306_DISPLAYALLON_RESUME,
+    SSD1306_NORMALDISPLAY,
+    SSD1306_DISPLAYON
+};
+/** Initialize a 128x32 SSD1306 oled display. */
+static const DevType MEM_TYPE Adafruit128x32 = {
+  Adafruit128x32init,
+  sizeof(Adafruit128x32init),
+  128,
+  32,
+  0
+};
+//------------------------------------------------------------------------------
+// This section is based on https://github.com/adafruit/Adafruit_SSD1306
+/** Initialization commands for a 128x64 SSD1306 oled display. */
+static const uint8_t MEM_TYPE Adafruit128x64init[] = {
+    // Init sequence for Adafruit 128x64 OLED module
+    0x00,                              // Set to command mode
+    SSD1306_DISPLAYOFF,
+    SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80
+    SSD1306_SETMULTIPLEX, 0x3F,        // ratio 64
+    SSD1306_SETDISPLAYOFFSET, 0x0,     // no offset
+    SSD1306_SETSTARTLINE | 0x0,        // line #0
+    SSD1306_CHARGEPUMP, 0x14,          // internal vcc
+    SSD1306_MEMORYMODE, 0x02,          // page mode
+    SSD1306_SEGREMAP | 0x1,            // column 127 mapped to SEG0
+    SSD1306_COMSCANDEC,                // column scan direction reversed
+    SSD1306_SETCOMPINS, 0x12,          // alt COM pins, disable remap
+    SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
+    SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
+    SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level
+    SSD1306_DISPLAYALLON_RESUME,
+    SSD1306_NORMALDISPLAY,
+    SSD1306_DISPLAYON
+};
+/** Initialize a 128x64 oled display. */
+static const DevType MEM_TYPE Adafruit128x64 = {
+  Adafruit128x64init,
+  sizeof(Adafruit128x64init),
+  128,
+  64,
+  0
+};
+//------------------------------------------------------------------------------
+// This section is based on https://github.com/stanleyhuangyc/MultiLCD
+/** Initialization commands for a 128x64 SH1106 oled display. */
+static const uint8_t MEM_TYPE SH1106_128x64init[] = {
+  0x00,                                  // Set to command mode
+  SSD1306_DISPLAYOFF,
+  SSD1306_SETSTARTPAGE | 0X0,            // set page address
+  SSD1306_SETCONTRAST, 0x80,             // 128
+  SSD1306_SEGREMAP | 0X1,                // set segment remap
+  SSD1306_NORMALDISPLAY,                 // normal / reverse
+  SSD1306_SETMULTIPLEX, 0x3F,            // ratio 64
+  SH1106_SET_PUMP_MODE, SH1106_PUMP_ON,  // set charge pump enable
+  SH1106_SET_PUMP_VOLTAGE | 0X2,         // 8.0 volts
+  SSD1306_COMSCANDEC,                    // Com scan direction
+  SSD1306_SETDISPLAYOFFSET, 0X00,        // set display offset
+  SSD1306_SETDISPLAYCLOCKDIV, 0X80,      // set osc division
+  SSD1306_SETPRECHARGE, 0X1F,            // set pre-charge period
+  SSD1306_SETCOMPINS, 0X12,              // set COM pins
+  SSD1306_SETVCOMDETECT,  0x40,          // set vcomh
+  SSD1306_DISPLAYON
+};
+/** Initialize a 128x64 oled SH1106 display. */
+static const DevType MEM_TYPE SH1106_128x64 =  {
+  SH1106_128x64init,
+  sizeof(SH1106_128x64init),
+  128,
+  64,
+  2    // SH1106 is a 132x64 controller.  Use middle 128 columns.
+};
+#endif  // SSD1306init_h
--- a/Sensors.cpp
+++ b/Sensors.cpp
@@ -184,7 +184,7 @@ void Sensor::load(){
  struct SensorData data;
  Sensor *tt;

-  for(uint16_t i=0;i<EEStore::eeStore->data.nSensors;i++){
+  for(int i=0;i<EEStore::eeStore->data.nSensors;i++){
    EEPROM.get(EEStore::pointer(),data);
    tt=create(data.snum,data.pin,data.pullUp);
    EEStore::advance(sizeof(tt->data));
--- a/Turnouts.cpp
+++ b/Turnouts.cpp
@@ -103,7 +103,7 @@ void Turnout::load(){
  struct TurnoutData data;
  Turnout *tt;

-  for(uint16_t i=0;i<EEStore::eeStore->data.nTurnouts;i++){
+  for(int i=0;i<EEStore::eeStore->data.nTurnouts;i++){
    EEPROM.get(EEStore::pointer(),data);
    if (data.tStatus & STATUS_PWM) tt=create(data.id,data.tStatus & STATUS_PWMPIN, data.inactiveAngle,data.moveAngle);
    else tt=create(data.id,data.address,data.subAddress);
--- a/WifiESP.cpp
+++ b/WifiESP.cpp
@@ -1,264 +0,0 @@
-/*
-    © 2021, Harald Barth.
-
-    This file is part of CommandStation-EX
-
-    This is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    It is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
-*/
-
-#include "defines.h"
-#ifdef ESP_FAMILY
-#include <ESP8266WiFi.h>
-#include <ESPAsyncTCP.h>
-#include <vector>
-#include <string>
-
-#include "WifiESP.h"
-#include "DIAG.h"
-#include "RingStream.h"
-#include "CommandDistributor.h"
-#include <string.h>
-
-static std::vector<AsyncClient*> clients; // a list to hold all clients
-static AsyncServer *server;
-
-static RingStream *outboundRing = new RingStream(2048);
-
-static void handleError(void* arg, AsyncClient* client, int8_t error) {
-  DIAG(F("connection error %s from client %s"), client->errorToString(error), client->remoteIP().toString().c_str());
-}
-
-static void handleData(void* arg, AsyncClient* client, void *data, size_t len) {
-  //DIAG(F("data received from client %s"), client->remoteIP().toString().c_str());
-  uint8_t clientId;
-  for (clientId=0; clientId<clients.size(); clientId++){
-    if (clients[clientId] == client) break;
-  }
-  if (clientId < clients.size()) {
-    byte cmd[len+1];
-    memcpy(cmd,data,len);
-    cmd[len]=0;
-    outboundRing->mark(clientId);
-    CommandDistributor::parse(clientId,cmd,outboundRing);
-    outboundRing->commit();
-  }
-}
-
-//static AsyncClient *debugclient = NULL;
-
-bool sendData(AsyncClient *client, char* data, size_t count) {
-  size_t willsend = 0;
-
-  // reply to client
-  if (client->canSend()) {
-    while (count > 0) {
-      if (client->connected())
-	willsend = client->add(data, count); // add checks for space()
-      else
-	willsend = 0;
-      if (willsend < count) {
-	DIAG(F("Willsend %d of count %d"), willsend, count);
-      }
-      if (client->connected() && client->send()) {
-	count = count - willsend;
-	data = data + willsend;
-      } else {
-	DIAG(F("Could not send promised %d"), count);
-	return false;
-      }
-    }
-    // Did send all bytes we wanted
-    return true;
-  }
-  DIAG(F("Aborting: Busy or space=0"));
-  return false;
-}
-
-static void deleteClient(AsyncClient* client) {
-  uint8_t clientId;
-  for (clientId=0; clientId<clients.size(); clientId++){
-    if (clients[clientId] == client) break;
-  }
-  if (clientId < clients.size()) {
-    clients[clientId] = NULL;
-  }
-}
-static void handleDisconnect(void* arg, AsyncClient* client) {
-  DIAG(F("Client disconnected"));
-  deleteClient(client);
-}
-
-static void handleTimeOut(void* arg, AsyncClient* client, uint32_t time) {
-  DIAG(F("client ACK timeout ip: %s"), client->remoteIP().toString().c_str());
-  deleteClient(client);
-}
-
-
-static void handleNewClient(void* arg, AsyncClient* client) {
-  DIAG(F("New client %s"), client->remoteIP().toString().c_str());
-
-  // add to list
-  clients.push_back(client);
-
-  // register events
-  client->onData(&handleData, NULL);
-  client->onError(&handleError, NULL);
-  client->onDisconnect(&handleDisconnect, NULL);
-  client->onTimeout(&handleTimeOut, NULL);
-
-}
-
-/*  Things one _might_ want to do:
-   Disable soft watchdog: ESP.wdtDisable()
-   Enable  soft watchdog: ESP.wdtEnable(X) ignores the value of X and enables it for fixed
-                          time at least in version 3.0.2 of the esp8266 package.
-
-Internet says:
-
-I manage to complety disable the hardware watchdog on ESP8266 in order to run the benchmark CoreMark. 
-
-void hw_wdt_disable(){
-  *((volatile uint32_t*) 0x60000900) &= ~(1); // Hardware WDT OFF
-}
-
-void hw_wdt_enable(){
-  *((volatile uint32_t*) 0x60000900) |= 1; // Hardware WDT ON
-}
-
-*/
-
-bool WifiESP::setup(const char *SSid,
-                    const char *password,
-                    const char *hostname,
-                    int port,
-                    const byte channel) {
-  bool havePassword = true;
-  bool haveSSID = true;
-  bool wifiUp = false;
-
-  // We are server and should not sleep
-  wifi_set_sleep_type(NONE_SLEEP_T);
-  // connects to access point
-
-  const char *yourNetwork = "Your network ";
-  if (strncmp(yourNetwork, SSid, 13) == 0 || strncmp("", SSid, 13) == 0)
-    haveSSID = false;
-  if (strncmp(yourNetwork, password, 13) == 0 || strncmp("", password, 13) == 0)
-    havePassword = false;
-  
-  if (haveSSID && havePassword) {
-    WiFi.mode(WIFI_STA);
-    WiFi.setAutoReconnect(true);
-    WiFi.begin(SSid, password);
-    while (WiFi.status() != WL_CONNECTED) {
-      Serial.print('.');
-      delay(500);
-    }
-    if (WiFi.status() == WL_CONNECTED) {
-      DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
-      wifiUp = true;
-    }
-  }
-  if (!haveSSID) {
-    // prepare all strings
-    String strSSID("DCC_");
-    String strPass("PASS_");
-    String strMac = WiFi.macAddress();
-    strMac.remove(0,9);
-    strMac.replace(":","");
-    strMac.replace(":","");
-    strSSID.concat(strMac);
-    strPass.concat(strMac);
-
-    WiFi.mode(WIFI_AP);
-    if (WiFi.softAP(strSSID.c_str(),
-		    havePassword ? password : strPass.c_str(),
-		    channel, false, 8)) {
-      DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
-      DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
-      wifiUp = true;
-    }
-  }
-
-
-  if (!wifiUp) {
-    DIAG(F("Wifi all fail"));
-    // no idea to go on
-    return false;
-  }
-
-  server = new AsyncServer(port); // start listening on tcp port
-
-  server->onClient(&handleNewClient, server);
-  server->begin();
-  DIAG(F("Server up port %d"),port);
-
-  return true;
-}
-
-void WifiESP::loop() {
-  AsyncClient *client = NULL;
-  // Do something with outboundRing
-  // call sendData
-  int clientId=outboundRing->peek();
-  if (clientId >= 0) {
-    if (clientId > clients.size()) {
-      // something is wrong with the ringbuffer position
-      outboundRing->info();
-      client = NULL;
-    } else {
-      client = clients[clientId];
-    }
-//    if (client != debugclient) {
-//      DIAG(F("new client pointer = %x from id %d"), client, clientId);
-//      debugclient = client;
-//    }
-  } else {
-    client = NULL;
-  }
-  if (clientId>=0 && client && client->connected() && client->canSend()) {
-    outboundRing->read();
-    int count=outboundRing->count();
-    //DIAG(F("Wifi reply client=%d, count=%d"), clientId,count);
-    {
-      char buffer[count+1];
-      for(int i=0;i<count;i++) {
-	int c = outboundRing->read();
-	if (c >= 0)
-	  buffer[i] = (char)c;
-	else {
-	  DIAG(F("Ringread fail at %d"),i);
-	  break;
-	}
-      }
-      buffer[count]=0;
-      //DIAG(F("SEND:%s COUNT:%d"),buffer,count);
-      uint8_t tries = 3;
-      while (! sendData(client, buffer, count)) {
-	DIAG(F("senData fail"));
-	yield();
-	if (tries == 0) break;
-      }
-    }
-  }
-#ifdef ESP_DEBUG
-  static unsigned long last = 0;
-  if (millis() - last > 60000) {
-    last = millis();
-    DIAG(F("+"));
-  }
-#endif
-  ESP.wdtFeed();
-}
-#endif //ESP_FAMILY
--- a/WifiESP.h
+++ b/WifiESP.h
@@ -1,37 +0,0 @@
-/*
- *  © 2021, Harald Barth.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-#ifndef WifiESP_h
-#define WifiESP_h
-
-#include "FSH.h"
-
-class WifiESP
-{
-
-public:
-  static bool setup(const char *wifiESSID,
-		    const char *wifiPassword,
-		    const char *hostname,
-		    const int port,
-		    const byte channel);
-  static void loop();
-private:
-};
-#endif
--- a/WifiInboundHandler.h
+++ b/WifiInboundHandler.h
@@ -1,22 +1,3 @@
-/*
- *  (c) 2021 Fred Decker.  All rights reserved.
- *  (c) 2020 Chris Harlow. All rights reserved.
- *
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
 #ifndef WifiInboundHandler_h
 #define WifiInboundHandler_h

--- a/config.example.h
+++ b/config.example.h
@@ -1,24 +1,8 @@
-/*
- *  COPYRIGHT (c) 2020 Fred Decker
- *  
- *  This file is part of CommandStation-EX
- *
- *  This is free software: you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation, either version 3 of the License, or
- *  (at your option) any later version.
- *
- *  It is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
- */
-
 /**********************************************************************

+config.h
+COPYRIGHT (c) 2020 Fred Decker
+
 The configuration file for DCC-EX Command Station

 **********************************************************************/
@@ -34,6 +18,7 @@ The configuration file for DCC-EX Command Station
 //
 //  STANDARD_MOTOR_SHIELD : Arduino Motor shield Rev3 based on the L298 with 18V 2A per channel
 //  POLOLU_MOTOR_SHIELD   : Pololu MC33926 Motor Driver (not recommended for prog track)
+//  POLOLU_TB9051FTG      : Pololu Dual TB9051FTG Motor Driver
 //  FUNDUMOTO_SHIELD      : Fundumoto Shield, no current sensing (not recommended, no short protection)
 //  FIREBOX_MK1           : The Firebox MK1                    
 //  FIREBOX_MK1S          : The Firebox MK1S
@@ -41,14 +26,7 @@ The configuration file for DCC-EX Command Station
 //   |
 //   +-----------------------v
 //
-//#define MOTOR_SHIELD_TYPE STANDARD_MOTOR_SHIELD
-
-#define ESP_MOTOR_SHIELD F("ESP"),					\
- new MotorDriver(D3, D5, UNUSED_PIN, UNUSED_PIN, UNUSED_PIN, 2.99, 2000, UNUSED_PIN),\
- new MotorDriver(D2, D6, UNUSED_PIN, UNUSED_PIN, A0        , 2.99, 2000, UNUSED_PIN)
-
-#define MOTOR_SHIELD_TYPE ESP_MOTOR_SHIELD
-
+#define MOTOR_SHIELD_TYPE STANDARD_MOTOR_SHIELD
 /////////////////////////////////////////////////////////////////////////////////////
 //
 // The IP port to talk to a WIFI or Ethernet shield.
@@ -60,7 +38,7 @@ The configuration file for DCC-EX Command Station
 // NOTE: Only supported on Arduino Mega
 // Set to false if you not even want it on the Arduino Mega
 //
-//#define ENABLE_WIFI true
+#define ENABLE_WIFI true

 /////////////////////////////////////////////////////////////////////////////////////
 //
--- a/defines.h
+++ b/defines.h
@@ -1,5 +1,5 @@
 /*
-    © 2020,2021 Harald Barth.
+    © 2020, Harald Barth.

    This file is part of CommandStation-EX

@@ -18,20 +18,12 @@

 */

-////////////////////////////////////////////////////////////////////////////////
-//
-#if defined (ARDUINO_ARCH_ESP8266)
-#define ESP_FAMILY
-//#define ESP_DEBUG
-#define SLOW_ANALOG_READ
-#endif
-
 ////////////////////////////////////////////////////////////////////////////////
 //
 // WIFI_ON: All prereqs for running with WIFI are met
 // Note: WIFI_CHANNEL may not exist in early config.h files so is added here if needed.

-#if ENABLE_WIFI && (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO)  || defined(TEENSYDUINO) || defined (ESP_FAMILY))
+#if ENABLE_WIFI && (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO)  || defined(TEENSYDUINO))
 #define WIFI_ON true
 #ifndef WIFI_CHANNEL
 #define WIFI_CHANNEL 1
--- a/freeMemory.cpp
+++ b/freeMemory.cpp
@@ -27,8 +27,6 @@ extern "C" char* sbrk(int);
 #elif defined(__AVR__)
 extern char *__brkval;
 extern char *__malloc_heap_start;
-#elif defined(ARDUINO_ARCH_ESP8266)
-// supported but nothing needed here
 #else
 #error Unsupported board type
 #endif
@@ -36,7 +34,7 @@ extern char *__malloc_heap_start;

 static volatile int minimum_free_memory = __INT_MAX__;

-#if !defined(__IMXRT1062__) && !defined(ARDUINO_ARCH_ESP8266)
+#if !defined(__IMXRT1062__)
 static inline int freeMemory() {
  char top;
 #if defined(__arm__)
@@ -57,18 +55,7 @@ int minimumFreeMemory() {
  return retval;
 }

-#elif defined(ARDUINO_ARCH_ESP8266)
-// ESP8266
-static inline int freeMemory() {
-  return ESP.getFreeHeap();
-}
-// Return low memory value.
-int minimumFreeMemory() {
-  int retval = minimum_free_memory;
-  return retval;
-}
 #else
-// All types of TEENSYs
 #if defined(ARDUINO_TEENSY40)
  static const unsigned DTCM_START = 0x20000000UL;
  static const unsigned OCRAM_START = 0x20200000UL;
@@ -121,3 +108,4 @@ void updateMinimumFreeMemory(unsigned char extraBytes) {
  if (spare < 0) spare = 0;
  if (spare < minimum_free_memory) minimum_free_memory = spare;
 }
+
--- a/version.h
+++ b/version.h
@@ -3,14 +3,7 @@

 #include "StringFormatter.h"

-
-#define VERSION "3.1.6"
-// 3.1.6 Make output ID two bytes and guess format/size of registered outputs found in EEPROM
-// 3.1.5 Fix LCD corruption on power-up
-// 3.1.4 Refactor OLED and LCD drivers and remove unused code
-// 3.1.3 Add a loop delay to give more time for sensing an Ethernet cable connection
-// 3.1.2 Eliminate wait after write when prog is joined or prog power is off
-// 3.1.1 SH1106 OLED Display Offset Fix
+#define VERSION "3.1.0"
 // 3.0.16 Ignore CV1 bit 7 read rejected by decoder when identifying loco id.  
 // 3.0.15 only send function commands once, not 4 times
 // 3.0.14 gap in ack tolerant fix,  prog track power management over join fix.
Author	SHA1	Message	Date
Harald Barth	292e51afd3	add comment Pololu TB9051FTG	2022-07-04 22:50:19 +02:00
Harald Barth	42cda59109	add Pololu TB9051FTG	2022-07-04 22:42:04 +02:00
Harald Barth	ce892974ab	only set TCCR1A register for pins that are signal pins, done by calling setRailcomCutout() at right place instead of doing it for all pins in the timer code	2022-07-03 10:22:30 +02:00
Harald Barth	3f57c1210d	turn off signal pin during cutout as that might switch some H-bridges between high and low side	2022-05-17 08:06:10 +02:00
Harald Barth	c9195f8035	invert Railcom pulse if brake is inverted	2022-05-15 01:44:37 +02:00
Harald Barth	7fc5c48efa	remove not needed goto and volatile	2022-05-15 01:38:28 +02:00
Harald Barth	c245c27f5d	Repair railcom prototype to get signal on main and prog	2022-05-14 00:49:39 +02:00
Asbelos	67adf1e6c6	Merge branch 'master' into RailCon	2021-05-20 12:02:02 +01:00
Asbelos	8e71dd8926	Teensy double speed interrupt	2021-05-16 21:00:34 +01:00
Asbelos	955362a033	Merge branch 'master' into RailCon	2021-05-14 13:32:57 +01:00
Asbelos	4391b049d8	tidying, join and cmd	2021-05-14 13:32:20 +01:00
Asbelos	7e58165db9	First working	2021-05-11 16:33:12 +01:00
Asbelos	945af43500	Not working	2021-05-11 15:09:44 +01:00