From 8af74f70827f278e8f5732345cd4e210c4c9d50c Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sun, 30 Jan 2022 23:56:16 +0100
Subject: [PATCH 1/2] protect analog read with cli()

---
 MotorDriver.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index 35c4388..d65d2ea 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -152,16 +152,16 @@ int MotorDriver::getCurrentRaw() {
   bool irq = disableInterrupts();
   current = analogRead(currentPin)-senseOffset;
   enableInterrupts(irq);
-#elif defined(ARDUINO_TEENSY32) || defined(ARDUINO_TEENSY35)|| defined(ARDUINO_TEENSY36)
+#else // Uno, Mega and all the TEENSY3* but not TEENSY4* 
   unsigned char sreg_backup;
   sreg_backup = SREG;   /* save interrupt enable/disable state */
   cli();
   current = analogRead(currentPin)-senseOffset;
+#if defined(ARDUINO_TEENSY32) || defined(ARDUINO_TEENSY35)|| defined(ARDUINO_TEENSY36)
   overflow_count = 0;
-  SREG = sreg_backup;    /* restore interrupt state */
-#else
-  current = analogRead(currentPin)-senseOffset;
 #endif
+  if (sreg_backup & 128) sei();  /* restore interrupt state */
+#endif // outer #
   if (current<0) current=0-current;
   if ((faultPin != UNUSED_PIN)  && isLOW(fastFaultPin) && isHIGH(fastPowerPin))
       return (current == 0 ? -1 : -current);

From b6501c7e3e1398539299a16b45bd2908b95b08bd Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sun, 30 Jan 2022 23:58:34 +0100
Subject: [PATCH 2/2] revert to write ERROR

---
 DCCEXParser.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp
index a400225..f84c123 100644
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -886,7 +886,7 @@ void DCCEXParser::callback_R(int16_t result)
 void DCCEXParser::callback_Rloco(int16_t result) {
   const FSH * detail;
   if (result<=0) {
-    detail=F("<r ERROR %d>\n");
+    detail=F("<r %d>\n");
   } else {
     bool longAddr=result & LONG_ADDR_MARKER;        //long addr
     if (longAddr)